From 386e4f6d60c94ecc503ba41a0154f18fc41388f1 Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Mon, 19 May 2025 10:26:09 +0000 Subject: [PATCH 01/60] add ros2 pkgs --- ros2/graph_msf_ros2/CMakeLists.txt | 110 +++ .../config/core_extrinsic_params.yaml | 7 + .../config/core_graph_config.yaml | 4 + .../config/core_graph_params.yaml | 54 ++ .../graph_msf_ros2_py/__init__.py | 0 .../remove_map_and_odom_from_bag.py | 111 +++ .../bag_filter/remove_tf_from_bag.py | 56 ++ .../plot_latest_quantitites_in_folder.py | 177 +++++ .../graph_msf_ros2_py/replay/__init__.py | 0 .../manual_pose_files_to_tf_and_odom_bag.py | 37 + .../replay/transform_helpers.py | 156 ++++ .../utils/get_latest_time_string_in_folder.py | 17 + .../include/graph_msf_ros2/GraphMsfRos2.h | 174 +++++ .../include/graph_msf_ros2/GraphMsfRos2bu.h | 176 +++++ .../include/graph_msf_ros2/constants.h | 12 + .../graph_msf_ros2/extrinsics/ElementToRoot.h | 27 + .../extrinsics/StaticTransformsTf.h | 34 + .../extrinsics/StaticTransformsUrdf.h | 41 + .../graph_msf_ros2/ros/read_ros_params.h | 50 ++ .../include/graph_msf_ros2/util/conversions.h | 47 ++ ros2/graph_msf_ros2/package.xml | 41 + ros2/graph_msf_ros2/rviz/vis.rviz | 203 +++++ ros2/graph_msf_ros2/setup.py | 8 + ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp | 715 ++++++++++++++++++ .../graph_msf_ros2/src/lib/GraphMsfRos2bu.cpp | 167 ++++ .../src/lib/StaticTransformsTf.cpp | 64 ++ .../src/lib/StaticTransformsUrdf.cpp | 84 ++ ros2/graph_msf_ros2/src/lib/conversions.cpp | 98 +++ ros2/graph_msf_ros2/src/lib/readParams.cpp | 157 ++++ .../srv/OfflineOptimizationTrigger.srv | 4 + ros2/graph_msf_ros2_msgs/CMakeLists.txt | 18 + .../msg/Wgs84Coordinate.msg | 3 + ros2/graph_msf_ros2_msgs/package.xml | 26 + ros2/graph_msf_ros2_msgs/srv/GetPathInEnu.srv | 3 + 34 files changed, 2881 insertions(+) create mode 100644 ros2/graph_msf_ros2/CMakeLists.txt create mode 100644 ros2/graph_msf_ros2/config/core_extrinsic_params.yaml create mode 100644 ros2/graph_msf_ros2/config/core_graph_config.yaml create mode 100644 ros2/graph_msf_ros2/config/core_graph_params.yaml create mode 100644 ros2/graph_msf_ros2/graph_msf_ros2_py/__init__.py create mode 100644 ros2/graph_msf_ros2/graph_msf_ros2_py/bag_filter/remove_map_and_odom_from_bag.py create mode 100644 ros2/graph_msf_ros2/graph_msf_ros2_py/bag_filter/remove_tf_from_bag.py create mode 100644 ros2/graph_msf_ros2/graph_msf_ros2_py/plotting/plot_latest_quantitites_in_folder.py create mode 100644 ros2/graph_msf_ros2/graph_msf_ros2_py/replay/__init__.py create mode 100644 ros2/graph_msf_ros2/graph_msf_ros2_py/replay/manual_pose_files_to_tf_and_odom_bag.py create mode 100644 ros2/graph_msf_ros2/graph_msf_ros2_py/replay/transform_helpers.py create mode 100644 ros2/graph_msf_ros2/graph_msf_ros2_py/utils/get_latest_time_string_in_folder.py create mode 100644 ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h create mode 100644 ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2bu.h create mode 100644 ros2/graph_msf_ros2/include/graph_msf_ros2/constants.h create mode 100644 ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/ElementToRoot.h create mode 100644 ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsTf.h create mode 100644 ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsUrdf.h create mode 100644 ros2/graph_msf_ros2/include/graph_msf_ros2/ros/read_ros_params.h create mode 100644 ros2/graph_msf_ros2/include/graph_msf_ros2/util/conversions.h create mode 100644 ros2/graph_msf_ros2/package.xml create mode 100644 ros2/graph_msf_ros2/rviz/vis.rviz create mode 100644 ros2/graph_msf_ros2/setup.py create mode 100644 ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp create mode 100644 ros2/graph_msf_ros2/src/lib/GraphMsfRos2bu.cpp create mode 100644 ros2/graph_msf_ros2/src/lib/StaticTransformsTf.cpp create mode 100644 ros2/graph_msf_ros2/src/lib/StaticTransformsUrdf.cpp create mode 100644 ros2/graph_msf_ros2/src/lib/conversions.cpp create mode 100644 ros2/graph_msf_ros2/src/lib/readParams.cpp create mode 100644 ros2/graph_msf_ros2/srv/OfflineOptimizationTrigger.srv create mode 100644 ros2/graph_msf_ros2_msgs/CMakeLists.txt create mode 100644 ros2/graph_msf_ros2_msgs/msg/Wgs84Coordinate.msg create mode 100644 ros2/graph_msf_ros2_msgs/package.xml create mode 100644 ros2/graph_msf_ros2_msgs/srv/GetPathInEnu.srv diff --git a/ros2/graph_msf_ros2/CMakeLists.txt b/ros2/graph_msf_ros2/CMakeLists.txt new file mode 100644 index 00000000..ed46849d --- /dev/null +++ b/ros2/graph_msf_ros2/CMakeLists.txt @@ -0,0 +1,110 @@ +cmake_minimum_required(VERSION 3.16) +project(graph_msf_ros2) + +# Compile as C++17 +add_compile_options(-std=c++17) + +find_package(ament_cmake REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(urdf REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(std_srvs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(graph_msf REQUIRED) + +# Color (Optional) +if (NOT WIN32) + string(ASCII 27 Esc) + set(ColourReset "${Esc}[m") + set(BoldMagenta "${Esc}[1;35m") + set(Magenta "${Esc}[35m") +endif () + +# Include directories +include_directories( + include + ${EIGEN3_INCLUDE_DIR} + ${graph_msf_INCLUDE_DIRS} +) + +# Library +add_library(${PROJECT_NAME} SHARED + src/lib/StaticTransformsTf.cpp + src/lib/StaticTransformsUrdf.cpp + src/lib/GraphMsfRos2.cpp + src/lib/readParams.cpp + src/lib/conversions.cpp +) + +# Link dependencies +ament_target_dependencies(${PROJECT_NAME} + rclcpp + geometry_msgs + nav_msgs + sensor_msgs + std_msgs + tf2 + tf2_ros + urdf + kdl_parser + tf2_eigen + std_srvs + visualization_msgs + graph_msf + rosidl_default_runtime +) + +# Python Scripts ---------------------------------------------------------------- +install(PROGRAMS + graph_msf_ros2_py/replay/manual_pose_files_to_tf_and_odom_bag.py + graph_msf_ros2_py/plotting/plot_latest_quantitites_in_folder.py + graph_msf_ros2_py/bag_filter/remove_tf_from_bag.py + DESTINATION lib/${PROJECT_NAME} +) + +# Install headers, config, and other files -------------------------------------- +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} +) +install(DIRECTORY config rviz + DESTINATION share/${PROJECT_NAME} +) + +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}Targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# Export package +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies( + rclcpp + geometry_msgs + nav_msgs + sensor_msgs + std_msgs + tf2 + tf2_ros + urdf + kdl_parser + tf2_eigen + std_srvs + visualization_msgs + graph_msf + Eigen3 + rosidl_default_runtime +) + +ament_package() diff --git a/ros2/graph_msf_ros2/config/core_extrinsic_params.yaml b/ros2/graph_msf_ros2/config/core_extrinsic_params.yaml new file mode 100644 index 00000000..9d33da22 --- /dev/null +++ b/ros2/graph_msf_ros2/config/core_extrinsic_params.yaml @@ -0,0 +1,7 @@ +#External Prior/Transform Input +extrinsics: + worldFrame: world + mapFrame: map + odomFrame: odom + imuFrame: imu_box_link # IMU_CABIN_link #Frame of IMU used in integrator - Used for lookup transform to lidarFrame + initializeZeroYawAndPositionOfFrame: imu_box_link diff --git a/ros2/graph_msf_ros2/config/core_graph_config.yaml b/ros2/graph_msf_ros2/config/core_graph_config.yaml new file mode 100644 index 00000000..3b7434b9 --- /dev/null +++ b/ros2/graph_msf_ros2/config/core_graph_config.yaml @@ -0,0 +1,4 @@ +#Common +common_params: + verbosity: 0 #Debug Output, 0: only important events, 1: optimization duration, 2: added factors + reLocalizeWorldToMapAtStart: false #Whether the World->Map transform should be updated in the beginning (odom start at identity) diff --git a/ros2/graph_msf_ros2/config/core_graph_params.yaml b/ros2/graph_msf_ros2/config/core_graph_params.yaml new file mode 100644 index 00000000..bb912c7f --- /dev/null +++ b/ros2/graph_msf_ros2/config/core_graph_params.yaml @@ -0,0 +1,54 @@ +# Sensor Params +sensor_params: + imuRate: 100 #Rate of IMU input (Hz) + imuBufferLength: 250 + imuTimeOffset: 0.0 # Offset between IMU and LiDAR Measurements: can be determined with rqt_multiplot + +# Initialization +initialization_params: + estimateGravityFromImu: true # If true, the gravity is estimated in the beginning using the IMU, if false, the value of 9.81 is used + +# Factor Graph +graph_params: + useIsam: true # if false, then levenberg-marquardt is used, CURRENTLY NO EFFECT + smootherLag: 2.0 #Lag of fixed lag smoother[seconds], not needed for ISAM2 + additionalOptimizationIterations: 0 #Additional iterations of graph optimizer after update with new factors + findUnusedFactorSlots: true + enableDetailedResults: false + usingFallbackGraph: true + usingCholeskyFactorization: true # CHOLESKY faster, QR numerically more stable + usingBiasForPreIntegration: true + +# Outlier Rejection +outlier_params: + poseMotionOutlierThreshold: 0.3 # in meters, if jumping more than this, it is considered as absent GNSS, occurs between two GNSS measurements + +# Noise Parameters +noise_params: + ## IMU + ### Position + accNoiseDensity: 7.225e-08 #Continuous-time "Covariance" of accelerometer, microstrain: sigma^2=7.225e-7 + integrationNoiseDensity: 1.0e-03 #continuous-time "Covariance" describing integration uncertainty, default: 1.0e-06 + use2ndOrderCoriolis: true #Use 2nd order coriolis effect + ### Rotation + gyrNoiseDensity: 1.71e-08 #Continuous-time "Covariance" of gyroscope measurements, microstrain: sigma^2=1.71-08 + omegaCoriolis: 0.0 #Coriolis effect + ### Bias + accBiasRandomWalk: 1.0e-05 #gnss:1.0e-06 #lidar: 1e-01 #Continuous-time "Covariance" describing accelerometer bias random walk, default: 1.0e-06 + gyrBiasRandomWalk: 9.33e-08 #gnss:9.33e-08 #lidar: 9.33e-01 #Continuous-time "Covariance" describing gyroscope bias random walk, default: 9.33e-08 + biasAccOmegaInit: 1.0e-03 #covariance of bias used as initial estimate default: 1.0e-2 + accBiasPrior: 0.0 #Prior/starting value of accelerometer bias + gyrBiasPrior: 0.0 #Prior/starting value of gyroscope bias + +# Relinearization +relinearization_params: + positionReLinTh: 1.0e-02 #Position linearization threshold + rotationReLinTh: 1.0e-02 #Rotation linearization threshold + velocityReLinTh: 1.0e-02 #Linear Velocity linearization threshold + accBiasReLinTh: 1.0e-05 #Accelerometer bias linearization threshold + gyrBiasReLinTh: 1.0e-05 #Gyroscope bias linearization threshold + relinearizeSkip: 1 + enableRelinearization: true + evaluateNonlinearError: false + cacheLinearizedFactors: true + enablePartialRelinearizationCheck: false diff --git a/ros2/graph_msf_ros2/graph_msf_ros2_py/__init__.py b/ros2/graph_msf_ros2/graph_msf_ros2_py/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/ros2/graph_msf_ros2/graph_msf_ros2_py/bag_filter/remove_map_and_odom_from_bag.py b/ros2/graph_msf_ros2/graph_msf_ros2_py/bag_filter/remove_map_and_odom_from_bag.py new file mode 100644 index 00000000..5281510e --- /dev/null +++ b/ros2/graph_msf_ros2/graph_msf_ros2_py/bag_filter/remove_map_and_odom_from_bag.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 + +# Code taken from: https://gist.github.com/awesomebytes/51470efe54b45045c50263f56d7aec27 + +import sys +import rosbag2_py +from rclpy.serialization import deserialize_message +from rosidl_runtime_py.utilities import get_message + + +def filter_topics(in_bag, out_bag, frames_we_want): + reader = rosbag2_py.SequentialReader() + storage_options = rosbag2_py.StorageOptions(uri=in_bag, storage_id="sqlite3") + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format="cdr", output_serialization_format="cdr" + ) + + reader.open(storage_options, converter_options) + topic_types = reader.get_all_topics_and_types() + + type_map = {topic.name: topic.type for topic in topic_types} + + writer = rosbag2_py.SequentialWriter() + writer_storage_options = rosbag2_py.StorageOptions( + uri=out_bag, storage_id="sqlite3" + ) + writer.open(writer_storage_options, converter_options) + + for topic in topic_types: + writer.create_topic(topic) + + while reader.has_next(): + (topic, data, t) = reader.read_next() + + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + + if topic == "/tf" and hasattr(msg, "transforms"): + transforms_to_keep = [] + for transform in msg.transforms: + if ( + transform.header.frame_id in frames_we_want + and transform.child_frame_id in frames_we_want + ): + transforms_to_keep.append(transform) + + if transforms_to_keep: + msg.transforms = transforms_to_keep + writer.write(topic, data, t) + + elif topic != "/tf": + writer.write(topic, data, t) + + +def filter_topics_2(in_bag, out_bag, frames_we_dont_want): + reader = rosbag2_py.SequentialReader() + storage_options = rosbag2_py.StorageOptions(uri=in_bag, storage_id="sqlite3") + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format="cdr", output_serialization_format="cdr" + ) + + reader.open(storage_options, converter_options) + topic_types = reader.get_all_topics_and_types() + + type_map = {topic.name: topic.type for topic in topic_types} + + writer = rosbag2_py.SequentialWriter() + writer_storage_options = rosbag2_py.StorageOptions( + uri=out_bag, storage_id="sqlite3" + ) + writer.open(writer_storage_options, converter_options) + + for topic in topic_types: + writer.create_topic(topic) + + while reader.has_next(): + (topic, data, t) = reader.read_next() + + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + + if topic == "/tf" and hasattr(msg, "transforms"): + transforms_to_keep = [] + for transform in msg.transforms: + if ( + transform.header.frame_id not in frames_we_dont_want + and transform.child_frame_id not in frames_we_dont_want + ): + transforms_to_keep.append(transform) + + if transforms_to_keep: + msg.transforms = transforms_to_keep + writer.write(topic, data, t) + + elif topic != "/tf": + writer.write(topic, data, t) + + +if __name__ == "__main__": + print("Starting") + in_bag = sys.argv[1] + out_bag = sys.argv[2] + + # Use one of the filtering functions as needed + # Example: + # filter_topics(in_bag, out_bag, ['base_link', 'odom', 'map', 'torso', 'Hip', 'Pelvis', 'Tibia', 'base_footprint']) + filter_topics_2( + in_bag, out_bag, ["odom_graph_msf", "world_graph_msf", "map_o3d_gmsf"] + ) + + print("Done") diff --git a/ros2/graph_msf_ros2/graph_msf_ros2_py/bag_filter/remove_tf_from_bag.py b/ros2/graph_msf_ros2/graph_msf_ros2_py/bag_filter/remove_tf_from_bag.py new file mode 100644 index 00000000..04114575 --- /dev/null +++ b/ros2/graph_msf_ros2/graph_msf_ros2_py/bag_filter/remove_tf_from_bag.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python3 + +# Code taken from: https://gist.github.com/awesomebytes/51470efe54b45045c50263f56d7aec27 + +import sys +import rosbag2_py +from rclpy.serialization import deserialize_message, serialize_message +from rosidl_runtime_py.utilities import get_message + + +def filter_rosbag(input_bag_path, output_bag_path): + # Initialize the rosbag2 reader + reader = rosbag2_py.SequentialReader() + storage_options = rosbag2_py.StorageOptions( + uri=input_bag_path, storage_id="sqlite3" + ) + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format="cdr", output_serialization_format="cdr" + ) + reader.open(storage_options, converter_options) + + # Get all topics and types from the input bag + topic_types = reader.get_all_topics_and_types() + type_map = {topic.name: topic.type for topic in topic_types} + + # Initialize the rosbag2 writer + writer = rosbag2_py.SequentialWriter() + writer_storage_options = rosbag2_py.StorageOptions( + uri=output_bag_path, storage_id="sqlite3" + ) + writer.open(writer_storage_options, converter_options) + + # Create topics in the output bag based on the input bag's topics + for topic in topic_types: + writer.create_topic(topic) + + # Loop through each topic and message in the input rosbag + while reader.has_next(): + topic, data, timestamp = reader.read_next() + + # Skip the /tf topic + if topic == "/tf": + continue + + # Deserialize and re-serialize the message for writing + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + writer.write(topic, serialize_message(msg), timestamp) + + +if __name__ == "__main__": + print("Starting") + input_bag = sys.argv[1] + output_bag = sys.argv[2] + filter_rosbag(input_bag_path=input_bag, output_bag_path=output_bag) + print("Done") diff --git a/ros2/graph_msf_ros2/graph_msf_ros2_py/plotting/plot_latest_quantitites_in_folder.py b/ros2/graph_msf_ros2/graph_msf_ros2_py/plotting/plot_latest_quantitites_in_folder.py new file mode 100644 index 00000000..fa4fd243 --- /dev/null +++ b/ros2/graph_msf_ros2/graph_msf_ros2_py/plotting/plot_latest_quantitites_in_folder.py @@ -0,0 +1,177 @@ +#!/usr/bin/env python3 + +# General Packages +import os +import matplotlib.pyplot as plt +import numpy as np +import sys + +# Path to the folder containing the files +HOME_DIR = str(os.path.expanduser("~")) +dir_path = os.path.join( + HOME_DIR, + "workspaces/rsl_workspaces/graph_msf_ws/src/graph_msf_dev/examples/anymal_estimator_graph/logging", +) + +# Add the parent directory to sys.path +sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + +# Workspace +import utils.get_latest_time_string_in_folder as get_latest_time_string_in_folder + + +def plot_quantities_in_file( + file_path: str, type: str, dir_path: str, latest_file_string: str +): + # Data + fields = [] + if "bias" in type: + fields = ["t", "a_x", "a_y", "a_z", "w_x", "w_y", "w_z"] + elif "transform" in type or "pose" in type: + fields = ["t", "x", "y", "z", "qw", "qx", "qy", "qz", "roll", "pitch", "yaw"] + elif "velocity" in type: + fields = ["t", "v_x", "v_y", "v_z"] + + # Read + data = [] + first_line = True + with open(file_path, "r") as file: + for line in file: + # Header file + if first_line: + first_line = False + continue + # Assuming your file format is: timestamp,x,y,z,qw,qx,qy,qz + parts = line.strip().split(",") + + # Write to data + if len(parts) < len(fields): + continue # Skip malformed lines + data.append(np.asarray(parts)) + + # Process Data + data = np.array(data, dtype=np.float64) + # Subtract first timestamp + data[:, 0] = data[:, 0] - data[0, 0] + print(f"Data shape: {data.shape}") + print("First 5 rows of data: ", data[:5, :]) + # Sorty by time + # indices = np.argsort(data[:, 0]) + # data = data[indices, :] + # print(indices) + # Plot + plt.figure() + for i, field in enumerate(fields): + # Do not plot time field + if i == 0 or field == "qw" or field == "qx" or field == "qy" or field == "qz": + continue + else: + print(f"Plotting field: {field}") + # Create figure + plt.plot(data[:, 0], data[:, i], label=field) + + # Create plot + if ("transform" in type or "pose" in type) and field == "z": + plt.xlabel("Time [s]") + plt.ylabel("Position [m]") + plt.title(f"{type} Position Over Time") + plt.legend() + plt.grid() + # Save the plot + plot_path = os.path.join(dir_path, f"{latest_file_string}_plot_{type}_position.png") + plt.savefig(plot_path) + plt.figure() + elif ("transform" in type or "pose" in type) and field == "yaw": + plt.xlabel("Time [s]") + plt.ylabel("Orientation [rad]") + plt.title(f"{type} Orientation Over Time") + plt.legend() + plt.grid() + # Save the plot + plot_path = os.path.join(dir_path, f"{latest_file_string}_plot_{type}_orientation.png") + plt.savefig(plot_path) + plt.figure() + elif "velocity" in type and field == "v_z": + plt.xlabel("Time [s]") + plt.ylabel("Velocity [m/s]") + plt.title(f"{type} Velocity Over Time") + plt.legend() + plt.grid() + # Save the plot + plot_path = os.path.join(dir_path, f"{latest_file_string}_plot_{type}_velocity.png") + plt.savefig(plot_path) + elif "bias" in type and field == "a_z": + plt.xlabel("Time [s]") + plt.ylabel("Acceleration [m/s^2]") + plt.title(f"{type} Acceleration Over Time") + plt.legend() + plt.grid() + # Save the plot + plot_path = os.path.join(dir_path, f"{latest_file_string}_plot_{type}_acceleration.png") + plt.savefig(plot_path) + plt.figure() + elif "bias" in type and field == "w_z": + plt.xlabel("Time [s]") + plt.ylabel("Angular Velocity [rad/s]") + plt.title(f"{type} Angular Velocity Over Time") + plt.legend() + plt.grid() + # Save the plot + plot_path = os.path.join(dir_path, f"{latest_file_string}_plot_{type}_angular_velocity.png") + plt.savefig(plot_path) + else: # Continue plotting + continue + + print(f"Plot saved to: {plot_path}") + + # Now get the x, y plot + if "transform" in type or "pose" in type: + plt.plot(data[:, 1], data[:, 2], label="x-y") + plt.xlabel("x [m]") + plt.ylabel("y [m]") + plt.title(f"{type} x-y Position Over Time") + plt.legend() + plt.grid() + # Save the plot + plot_path = os.path.join(dir_path, f"{latest_file_string}_plot_{type}_x_y_position.png") + plt.savefig(plot_path) + print(f"Plot saved to: {plot_path}") + + + +def main(): + # We want to only get the latest files + latest_file_string = ( + get_latest_time_string_in_folder.get_latest_time_string_in_folder(dir_path) + ) + print("Latest file string: ", latest_file_string) + # Plot following files: + file_types = [ + "B_imu_bias", + "R_6D_transform", + "v_state_3D_velocity", + "X_state_6D_pose", + ] + # Get all files in the folder that contain the latest file string + files = os.listdir(dir_path) + files = [f for f in files if (latest_file_string in f and f.endswith(".csv"))] + print(f"Files containing {latest_file_string}: {files}") + + # Iterate through files and check if they contain the file types + for file in files: + for file_type in file_types: + if file_type in file: + print(f"Plotting {file_type} from file: {file}") + # Plot the quantities in the file + plot_quantities_in_file( + os.path.join(dir_path, file), + file_type, + dir_path, + latest_file_string, + ) + + +if __name__ == "__main__": + print("Starting") + main() + print("Done") diff --git a/ros2/graph_msf_ros2/graph_msf_ros2_py/replay/__init__.py b/ros2/graph_msf_ros2/graph_msf_ros2_py/replay/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/ros2/graph_msf_ros2/graph_msf_ros2_py/replay/manual_pose_files_to_tf_and_odom_bag.py b/ros2/graph_msf_ros2/graph_msf_ros2_py/replay/manual_pose_files_to_tf_and_odom_bag.py new file mode 100644 index 00000000..0497d71a --- /dev/null +++ b/ros2/graph_msf_ros2/graph_msf_ros2_py/replay/manual_pose_files_to_tf_and_odom_bag.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python + +# Imports +import os.path +from pathlib import Path + +HOME_DIR = str(Path.home()) + +# Workspace +from graph_msf_ros2_py.replay.transform_helpers import write_bag +from graph_msf_ros2_py.utils.get_latest_time_string_in_folder import ( + get_latest_time_string_in_folder, +) + +# Input and output files +dir_path = os.path.join( + HOME_DIR, + "workspaces/rsl_workspaces/graph_msf_ws/src/graph_msf_dev/examples/anymal_estimator_graph/logging", +) +latest_time_string = get_latest_time_string_in_folder(folder_path=dir_path) +input_file_path = os.path.join( + dir_path, latest_time_string + "_optimized_X_state_transform.csv" +) +output_bag_path = os.path.join( + dir_path, latest_time_string + "_bag_X_state_transform.bag" +) +fixed_frame_id = "offline_world_graph_msf" +child_frame_id = "imu_link" + + +def main(): + # Go through the input file and write the data to the output bag + write_bag(input_file_path, output_bag_path, fixed_frame_id, child_frame_id) + + +if __name__ == "__main__": + main() diff --git a/ros2/graph_msf_ros2/graph_msf_ros2_py/replay/transform_helpers.py b/ros2/graph_msf_ros2/graph_msf_ros2_py/replay/transform_helpers.py new file mode 100644 index 00000000..c6b6d80d --- /dev/null +++ b/ros2/graph_msf_ros2/graph_msf_ros2_py/replay/transform_helpers.py @@ -0,0 +1,156 @@ +#!/usr/bin/env python3 + +# General Packages +from scipy.spatial.transform import Rotation as R +import copy +import sys + +# ROS 2 +import rclpy +from rclpy.time import Time +from rclpy.serialization import serialize_message +from geometry_msgs.msg import TransformStamped +from tf2_msgs.msg import TFMessage +import rosbag2_py + + +def invert_transform(tf_msg): + # Old orientation + q_old = [ + tf_msg.transform.rotation.x, + tf_msg.transform.rotation.y, + tf_msg.transform.rotation.z, + tf_msg.transform.rotation.w, + ] + # Invert the orientation + r_old = R.from_quat(q_old) + r_new = r_old.inv() + q_new = r_new.as_quat() + + # Old translation + t_old = [ + tf_msg.transform.translation.x, + tf_msg.transform.translation.y, + tf_msg.transform.translation.z, + ] + t_new = -r_new.apply(t_old) + + # Define the new transform + tf_msg_new = TransformStamped() + tf_msg_new.header = copy.deepcopy(tf_msg.header) + tf_msg_new.header.frame_id = tf_msg.child_frame_id + tf_msg_new.child_frame_id = tf_msg.header.frame_id + + # Set the new translation + tf_msg_new.transform.translation.x = t_new[0] + tf_msg_new.transform.translation.y = t_new[1] + tf_msg_new.transform.translation.z = t_new[2] + + # Set the new orientation + tf_msg_new.transform.rotation.x = q_new[0] + tf_msg_new.transform.rotation.y = q_new[1] + tf_msg_new.transform.rotation.z = q_new[2] + tf_msg_new.transform.rotation.w = q_new[3] + + return tf_msg_new + + +def create_transform_stamped( + timestamp: float, + x: float, + y: float, + z: float, + qw: float, + qx: float, + qy: float, + qz: float, + fixed_frame_id: str, + child_frame_id: str, +): + t = TransformStamped() + t.header.stamp = Time(seconds=float(timestamp)).to_msg() + t.transform.translation.x = float(x) + t.transform.translation.y = float(y) + t.transform.translation.z = float(z) + t.transform.rotation.x = float(qx) + t.transform.rotation.y = float(qy) + t.transform.rotation.z = float(qz) + t.transform.rotation.w = float(qw) + t.header.frame_id = fixed_frame_id + t.child_frame_id = child_frame_id + return t + + +def write_bag(input_file_path, output_bag_path, fixed_frame_id, child_frame_id): + first_line = True + writer = rosbag2_py.SequentialWriter() + storage_options = rosbag2_py.StorageOptions( + uri=output_bag_path, storage_id="sqlite3" + ) + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format="cdr", output_serialization_format="cdr" + ) + writer.open(storage_options, converter_options) + + # Set the topic information for /tf + tf_topic_info = rosbag2_py.TopicMetadata( + name="/tf", type="tf2_msgs/msg/TFMessage", serialization_format="cdr" + ) + writer.create_topic(tf_topic_info) + + with open(input_file_path, "r") as file: + for line in file: + # Header line + if first_line: + first_line = False + continue + + # Assuming format: timestamp,x,y,z,qw,qx,qy,qz + parts = line.strip().split(",") + if len(parts) < 11: + continue # Skip malformed lines + timestamp, x, y, z, qx, qy, qz, qw, roll, pitch, yaw = parts + tf_msg = create_transform_stamped( + timestamp=timestamp, + x=x, + y=y, + z=z, + qw=qw, + qx=qx, + qy=qy, + qz=qz, + fixed_frame_id=fixed_frame_id, + child_frame_id=child_frame_id, + ) + + # Invert the transform + inv_tf = invert_transform(tf_msg) + + # Wrap in TFMessage + tf_message = TFMessage(transforms=[inv_tf]) + + # Write to the bag file + writer.write( + "/tf", + serialize_message(tf_message), + Time(seconds=float(timestamp)).nanoseconds, + ) + + print(f"Conversion complete. Output saved to {output_bag_path}") + + +if __name__ == "__main__": + rclpy.init() + if len(sys.argv) < 5: + print( + "Usage: python3 convert_ros2_bag_with_transform.py " + ) + sys.exit(1) + + input_file = sys.argv[1] + output_bag = sys.argv[2] + fixed_frame_id = sys.argv[3] + child_frame_id = sys.argv[4] + + write_bag(input_file, output_bag, fixed_frame_id, child_frame_id) + rclpy.shutdown() diff --git a/ros2/graph_msf_ros2/graph_msf_ros2_py/utils/get_latest_time_string_in_folder.py b/ros2/graph_msf_ros2/graph_msf_ros2_py/utils/get_latest_time_string_in_folder.py new file mode 100644 index 00000000..f9383ebd --- /dev/null +++ b/ros2/graph_msf_ros2/graph_msf_ros2_py/utils/get_latest_time_string_in_folder.py @@ -0,0 +1,17 @@ +#!/usr/bin/env python + +# Imports +import os + +def get_latest_time_string_in_folder(folder_path: str) -> str: + print(f"All files located at: {folder_path}:") + # Load all files in location + files = os.listdir(folder_path) + # Only get csv files + files = [f for f in files if f.endswith(".csv")] + # We want to only get the latest files + files.sort() + latest_filename = files[-1] + # Get first part of string before "_optimized" + time_string = latest_filename.split("_v")[0] + return time_string \ No newline at end of file diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h new file mode 100644 index 00000000..8327d603 --- /dev/null +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h @@ -0,0 +1,174 @@ +#ifndef GRAPHMSFROS_H +#define GRAPHMSFROS_H + +// std +#include +#include +#include +#include + +// ROS2 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Workspace +#include "graph_msf/interface/GraphMsf.h" +#include "graph_msf/interface/GraphMsfClassic.h" +#include "graph_msf/interface/GraphMsfHolistic.h" +// #include "graph_msf_ros/srv/offline_optimization_trigger.hpp" + +// Macros +#define ROS_QUEUE_SIZE 1 + +namespace graph_msf { + +class GraphMsfRos2 : public GraphMsfClassic, public GraphMsfHolistic { + public: + GraphMsfRos2(std::shared_ptr& node); + // Destructor + ~GraphMsfRos2() override = default; + + // Setup + void setup(std::shared_ptr staticTransformsPtr); + + protected: + // Functions that need implementation + virtual void initializePublishers(); + virtual void initializeSubscribers(); + virtual void initializeMessages(); + // virtual void initializeServices(rclcpp::Node& node); + + // Commodity Functions to be shared ----------------------------------- + // Static + static void addToPathMsg(const nav_msgs::msg::Path::SharedPtr& pathPtr, const std::string& frameName, + const rclcpp::Time& stamp, const Eigen::Vector3d& t, int maxBufferLength); + static void addToOdometryMsg( + const nav_msgs::msg::Odometry::SharedPtr& msgPtr, const std::string& fixedFrame, const std::string& movingFrame, + const rclcpp::Time& stamp, const Eigen::Isometry3d& T, const Eigen::Vector3d& W_v_W_F, + const Eigen::Vector3d& W_w_W_F, + const Eigen::Matrix& poseCovariance = Eigen::Matrix::Zero(), + const Eigen::Matrix& twistCovariance = Eigen::Matrix::Zero()); + static void addToPoseWithCovarianceStampedMsg( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr& msgPtr, const std::string& frameName, + const rclcpp::Time& stamp, const Eigen::Isometry3d& T, + const Eigen::Matrix& transformCovariance = Eigen::Matrix::Zero()); + static void extractCovariancesFromOptimizedState( + Eigen::Matrix& poseCovarianceRos, Eigen::Matrix& twistCovarianceRos, + const std::shared_ptr& optimizedStateWithCovarianceAndBiasPtr); + static void createVelocityMarker(const std::string& referenceFrameName, const rclcpp::Time& stamp, + const Eigen::Vector3d& velocity, visualization_msgs::msg::Marker& marker); + void createContactMarker(const std::string& referenceFrameName, const rclcpp::Time& stamp, + const Eigen::Vector3d& position, const std::string& nameSpace, const int id, + visualization_msgs::msg::Marker& marker); + + // Parameter Loading ----------------------------------- + virtual void readParams(); + + // Callbacks + virtual void imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuPtr); + + // Services + // bool srvOfflineSmootherOptimizeCallback(const + // std::shared_ptr req, + // std::shared_ptr + // res); + + // Publishing ----------------------------------- + virtual void publishState( + const std::shared_ptr& integratedNavStatePtr, + const std::shared_ptr& optimizedStateWithCovarianceAndBiasPtr); + void publishNonTimeCriticalData( + const Eigen::Matrix poseCovarianceRos, const Eigen::Matrix twistCovarianceRos, + const Eigen::Vector3d positionVarianceRos, const Eigen::Vector3d orientationVarianceRos, + const std::shared_ptr integratedNavStatePtr, + const std::shared_ptr optimizedStateWithCovarianceAndBiasPtr); + void publishOptimizedStateAndBias( + const std::shared_ptr optimizedStateWithCovarianceAndBiasPtr, + const Eigen::Matrix& poseCovarianceRos, const Eigen::Matrix& twistCovarianceRos); + + void publishTfTreeTransform(const std::string& frameName, const std::string& childFrameName, double timeStamp, + const Eigen::Isometry3d& T_frame_childFrame); + void publishImuOdoms(const std::shared_ptr& preIntegratedNavStatePtr, + const Eigen::Matrix& poseCovarianceRos, + const Eigen::Matrix& twistCovarianceRos) const; + void publishDiagVarianceVectors(const Eigen::Vector3d& posVarianceRos, const Eigen::Vector3d& rotVarianceRos, + const double timeStamp) const; + void publishVelocityMarkers(const std::shared_ptr& navStatePtr) const; + void publishImuPaths(const std::shared_ptr& navStatePtr) const; + void publishAddedImuMeas(const Eigen::Matrix& addedImuMeas, const rclcpp::Time& stamp) const; + + // Measure time + long secondsSinceStart(); + + // Node + std::shared_ptr node_; + + // Time + std::chrono::time_point startTime; + std::chrono::time_point currentTime; + + // Publishers + // TF + std::shared_ptr tfBroadcaster_; + + // Members + std::string referenceFrameAlignedNameId = "_graph_msf_aligned"; + std::string sensorFrameCorrectedNameId = "_graph_msf_corrected"; + std::string optimizationResultLoggingPath = ""; + + private: + // Publishers + rclcpp::Publisher::SharedPtr pubEstOdomImu_; + rclcpp::Publisher::SharedPtr pubEstWorldImu_; + rclcpp::Publisher::SharedPtr pubOptWorldImu_; + rclcpp::Publisher::SharedPtr pubEstWorldPosVariance_; + rclcpp::Publisher::SharedPtr pubEstWorldRotVariance_; + rclcpp::Publisher::SharedPtr pubVelocityMarker_; + rclcpp::Publisher::SharedPtr pubEstOdomImuPath_; + rclcpp::Publisher::SharedPtr pubEstWorldImuPath_; + rclcpp::Publisher::SharedPtr pubOptWorldImuPath_; + rclcpp::Publisher::SharedPtr pubAccelBias_; + rclcpp::Publisher::SharedPtr pubGyroBias_; + rclcpp::Publisher::SharedPtr pubMeasWorldGnssLPath_; + rclcpp::Publisher::SharedPtr pubMeasWorldGnssRPath_; + rclcpp::Publisher::SharedPtr pubMeasWorldLidarPath_; + rclcpp::Publisher::SharedPtr pubAddedImuMeas_; + + std::map::SharedPtr> + pubPoseStampedByTopicMap_ = {}; + + // Subscribers + rclcpp::Subscription::SharedPtr subImu_; + + // Messages + nav_msgs::msg::Odometry::SharedPtr estOdomImuMsgPtr_; + nav_msgs::msg::Odometry::SharedPtr estWorldImuMsgPtr_; + nav_msgs::msg::Odometry::SharedPtr optWorldImuMsgPtr_; + geometry_msgs::msg::Vector3Stamped::SharedPtr estWorldPosVarianceMsgPtr_; + geometry_msgs::msg::Vector3Stamped::SharedPtr estWorldRotVarianceMsgPtr_; + nav_msgs::msg::Path::SharedPtr estOdomImuPathPtr_; + nav_msgs::msg::Path::SharedPtr estWorldImuPathPtr_; + nav_msgs::msg::Path::SharedPtr optWorldImuPathPtr_; + nav_msgs::msg::Path::SharedPtr measWorldLidarPathPtr_; + geometry_msgs::msg::Vector3Stamped::SharedPtr accelBiasMsgPtr_; + geometry_msgs::msg::Vector3Stamped::SharedPtr gyroBiasMsgPtr_; + + // Services + // rclcpp::Service::SharedPtr srvSmootherOptimize_; + + // Last Optimized State Timestamp + double lastOptimizedStateTimestamp_ = 0.0; + + // Mutex + std::mutex rosPublisherMutex_; +}; +} // namespace graph_msf + +#endif // end GRAPHMSFROS_H diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2bu.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2bu.h new file mode 100644 index 00000000..69f012cf --- /dev/null +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2bu.h @@ -0,0 +1,176 @@ +/* +Copyright 2024 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +#pragma once +// std +#include +#include +#include +#include + +// ROS 2 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Workspace +#include "graph_msf/interface/GraphMsf.h" +#include "graph_msf/interface/GraphMsfClassic.h" +#include "graph_msf/interface/GraphMsfHolistic.h" +// #include "graph_msf_ros2/srv/OfflineOptimizationTrigger.hpp" + +// Macros +#define ROS_QUEUE_SIZE 1 + +namespace graph_msf { + +class GraphMsfRos2 : public GraphMsfClassic, public GraphMsfHolistic { + public: + explicit GraphMsfRos2(const std::shared_ptr& node); + ~GraphMsfRos2() override = default; + + // Setup + void setup(const std::shared_ptr& staticTransformsPtr); + + protected: + // Functions that need implementation + virtual void initializePublishers(); + virtual void initializeSubscribers(); + virtual void initializeMessages(); + // virtual void initializeServices(rclcpp::Node& node); + + // Commodity Functions to be shared ----------------------------------- + static void addToPathMsg(const std::shared_ptr& pathPtr, const std::string& frameName, + const rclcpp::Time& stamp, const Eigen::Vector3d& t, int maxBufferLength); + static void addToOdometryMsg( + const std::shared_ptr& msgPtr, const std::string& fixedFrame, + const std::string& movingFrame, const rclcpp::Time& stamp, const Eigen::Isometry3d& T, + const Eigen::Vector3d& W_v_W_F, const Eigen::Vector3d& W_w_W_F, + const Eigen::Matrix& poseCovariance = Eigen::Matrix::Zero(), + const Eigen::Matrix& twistCovariance = Eigen::Matrix::Zero()); + static void addToPoseWithCovarianceStampedMsg( + const std::shared_ptr& msgPtr, const std::string& frameName, + const rclcpp::Time& stamp, const Eigen::Isometry3d& T, + const Eigen::Matrix& transformCovariance = Eigen::Matrix::Zero()); + + static void extractCovariancesFromOptimizedState( + Eigen::Matrix& poseCovarianceRos, Eigen::Matrix& twistCovarianceRos, + const std::shared_ptr& optimizedStateWithCovarianceAndBiasPtr); + + static void createVelocityMarker(const std::string& referenceFrameName, const rclcpp::Time& stamp, + const Eigen::Vector3d& velocity, visualization_msgs::msg::Marker& marker); + void createContactMarker(const std::string& referenceFrameName, const rclcpp::Time& stamp, + const Eigen::Vector3d& position, const std::string& nameSpace, const int id, + visualization_msgs::msg::Marker& marker); + + // Parameter Loading ----------------------------------- + virtual void readParams(const rclcpp::Node& node); + + // Callbacks + virtual void imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuPtr); + + // Services + // bool srvOfflineSmootherOptimizeCallback( + // const std::shared_ptr req, + // std::shared_ptr res); + + // Publishing ----------------------------------- + virtual void publishState( + const std::shared_ptr& integratedNavStatePtr, + const std::shared_ptr& optimizedStateWithCovarianceAndBiasPtr); + void publishNonTimeCriticalData( + const Eigen::Matrix poseCovarianceRos, const Eigen::Matrix twistCovarianceRos, + const Eigen::Vector3d positionVarianceRos, const Eigen::Vector3d orientationVarianceRos, + const std::shared_ptr integratedNavStatePtr, + const std::shared_ptr optimizedStateWithCovarianceAndBiasPtr); + void publishOptimizedStateAndBias( + const std::shared_ptr optimizedStateWithCovarianceAndBiasPtr, + const Eigen::Matrix& poseCovarianceRos, const Eigen::Matrix& twistCovarianceRos); + + void publishTfTreeTransform(const std::string& frameName, const std::string& childFrameName, double timeStamp, + const Eigen::Isometry3d& T_frame_childFrame); + void publishImuOdoms(const std::shared_ptr& preIntegratedNavStatePtr, + const Eigen::Matrix& poseCovarianceRos, + const Eigen::Matrix& twistCovarianceRos) const; + void publishDiagVarianceVectors(const Eigen::Vector3d& posVarianceRos, const Eigen::Vector3d& rotVarianceRos, + const double timeStamp) const; + void publishVelocityMarkers(const std::shared_ptr& navStatePtr) const; + void publishImuPaths(const std::shared_ptr& navStatePtr) const; + void publishAddedImuMeas(const Eigen::Matrix& addedImuMeas, const rclcpp::Time& stamp) const; + + // Measure time + long secondsSinceStart(); + + // Node + std::shared_ptr node_; + + // Time + std::chrono::time_point startTime; + std::chrono::time_point currentTime; + + // Publishers + tf2_ros::TransformBroadcaster tfBroadcaster; + + // Members + std::string referenceFrameAlignedNameId = "_graph_msf_aligned"; + std::string sensorFrameCorrectedNameId = "_graph_msf_corrected"; + std::string optimizationResultLoggingPath = ""; + + private: + // Publishers + rclcpp::Publisher::SharedPtr pubEstOdomImu_; + rclcpp::Publisher::SharedPtr pubEstWorldImu_; + rclcpp::Publisher::SharedPtr pubOptWorldImu_; + rclcpp::Publisher::SharedPtr pubEstWorldPosVariance_; + rclcpp::Publisher::SharedPtr pubEstWorldRotVariance_; + rclcpp::Publisher::SharedPtr pubVelocityMarker_; + rclcpp::Publisher::SharedPtr pubEstOdomImuPath_; + rclcpp::Publisher::SharedPtr pubEstWorldImuPath_; + rclcpp::Publisher::SharedPtr pubOptWorldImuPath_; + rclcpp::Publisher::SharedPtr pubMeasWorldGnssLPath_; + rclcpp::Publisher::SharedPtr pubMeasWorldGnssRPath_; + rclcpp::Publisher::SharedPtr pubMeasWorldLidarPath_; + rclcpp::Publisher::SharedPtr pubAccelBias_; + rclcpp::Publisher::SharedPtr pubGyroBias_; + rclcpp::Publisher::SharedPtr pubAddedImuMeas_; + + std::map::SharedPtr> + pubPoseStampedByTopicMap_; + + // Subscribers + rclcpp::Subscription::SharedPtr subImu_; + + // Messages + std::shared_ptr estOdomImuMsgPtr_; + std::shared_ptr estWorldImuMsgPtr_; + std::shared_ptr optWorldImuMsgPtr_; + std::shared_ptr estWorldPosVarianceMsgPtr_; + std::shared_ptr estWorldRotVarianceMsgPtr_; + std::shared_ptr estOdomImuPathPtr_; + std::shared_ptr estWorldImuPathPtr_; + std::shared_ptr optWorldImuPathPtr_; + std::shared_ptr measWorldLidarPathPtr_; + std::shared_ptr accelBiasMsgPtr_; + std::shared_ptr gyroBiasMsgPtr_; + + // Service server + // rclcpp::Service::SharedPtr srvSmootherOptimize_; + + // Last Optimized State Timestamp + double lastOptimizedStateTimestamp_ = 0.0; + + // Mutex + std::mutex rosPublisherMutex_; +}; + +} // namespace graph_msf diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/constants.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/constants.h new file mode 100644 index 00000000..f73f82fc --- /dev/null +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/constants.h @@ -0,0 +1,12 @@ +/* +Copyright 2023 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +#include "graph_msf/interface/Terminal.h" + +#pragma once + +#define REGULAR_COUT std::cout << YELLOW_START << "GraphMsfRos2" << COLOR_END diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/ElementToRoot.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/ElementToRoot.h new file mode 100644 index 00000000..0dfc2212 --- /dev/null +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/ElementToRoot.h @@ -0,0 +1,27 @@ +/* +Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ +#pragma once + +#include +#include + +namespace graph_msf +{ + + class ElementToRoot final + { + public: + /// Constructor + explicit ElementToRoot(const tf2::Transform &T, const std::string &rootName_, const std::string &elementName_) + : T_root_element(T), rootName(rootName_), elementName(elementName_) {} + + tf2::Transform T_root_element; ///< The tf2 Transform from root to element + std::string rootName; ///< The name of the root element to which this link is attached + std::string elementName; ///< The name of the element + }; + +} // namespace graph_msf diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsTf.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsTf.h new file mode 100644 index 00000000..a79e2374 --- /dev/null +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsTf.h @@ -0,0 +1,34 @@ +/* +Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +#pragma once +// ROS 2 +#include +#include +#include + +// Workspace +#include "graph_msf/config/StaticTransforms.h" + +namespace graph_msf { + +class StaticTransformsTf : public StaticTransforms { + public: + explicit StaticTransformsTf(const rclcpp::Node::SharedPtr& node, + const graph_msf::StaticTransforms& staticTransforms); + + virtual ~StaticTransformsTf() = default; + + protected: + void findTransformations() override; + + // Members + std::shared_ptr tf_buffer_; + tf2_ros::TransformListener tf_listener_; +}; + +} // namespace graph_msf diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsUrdf.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsUrdf.h new file mode 100644 index 00000000..d31357b8 --- /dev/null +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsUrdf.h @@ -0,0 +1,41 @@ +/* +Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +#ifndef StaticTransformsUrdf_H +#define StaticTransformsUrdf_H + +#include +#include +#include +#include "graph_msf/config/StaticTransforms.h" +#include "graph_msf_ros2/extrinsics/ElementToRoot.h" +#include "rclcpp/rclcpp.hpp" +#include "urdf/model.h" + +namespace graph_msf { + +class StaticTransformsUrdf : public StaticTransforms { + public: + StaticTransformsUrdf(const std::shared_ptr privateNodePtr); + void setup(); + + protected: + std::string urdfDescriptionName_; + urdf::Model urdfModel_; + std::unique_ptr model_; + KDL::Tree tree_; + std::map segments_; + void getRootTransformations(const KDL::SegmentMap::const_iterator element, std::string rootName = ""); + tf2::Transform kdlToTransform(const KDL::Frame& k); + std::shared_ptr privateNode_; + + private: + virtual void findTransformations() = 0; +}; + +} // namespace graph_msf +#endif // end StaticTransformsUrdf_H diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/ros/read_ros_params.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/ros/read_ros_params.h new file mode 100644 index 00000000..0d3af927 --- /dev/null +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/ros/read_ros_params.h @@ -0,0 +1,50 @@ +#pragma once + +// ROS 2 +#include +#include +#include +#include + +// Workspace +#include "graph_msf/interface/Terminal.h" + +namespace graph_msf { + +template +inline void printKey(const std::string& key, T value) { + std::cout << YELLOW_START << "GraphMsfRos2 " << COLOR_END << key << " set to: " << value << std::endl; +} + +template <> +inline void printKey(const std::string& key, std::vector vector) { + std::cout << YELLOW_START << "GraphMsfRos2 " << COLOR_END << key << " set to: "; + for (const auto& element : vector) { + std::cout << element << ","; + } + std::cout << std::endl; +} + +// Implementation of Templating +template +T tryGetParam(std::shared_ptr node, const std::string& key) { + T value; + RCLCPP_INFO(node->get_logger(), "Attempting to retrieve parameter: %s", key.c_str()); + + if (node->get_parameter(key, value)) { + printKey(key, value); + RCLCPP_INFO(node->get_logger(), "Successfully retrieved parameter: %s", key.c_str()); + return value; + } + + if (node->get_parameter("/" + key, value)) { + printKey("/" + key, value); + RCLCPP_INFO(node->get_logger(), "Successfully retrieved parameter with absolute key: %s", ("/" + key).c_str()); + return value; + } + + RCLCPP_ERROR(node->get_logger(), "Parameter not found: %s", key.c_str()); + throw std::runtime_error("GraphMsfRos2 - " + key + " not specified."); +} + +} // namespace graph_msf diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/util/conversions.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/util/conversions.h new file mode 100644 index 00000000..f94d773f --- /dev/null +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/util/conversions.h @@ -0,0 +1,47 @@ +/* +Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace graph_msf { + +Eigen::Matrix convertCovarianceGtsamConventionToRosConvention( + const Eigen::Matrix& covGtsam); + +void odomMsgToEigen(const nav_msgs::msg::Odometry& odomLidar, Eigen::Matrix4d& T); + +void geometryPoseToEigen(const geometry_msgs::msg::Pose& pose, Eigen::Matrix4d& T); + +void geometryPoseToEigen(const geometry_msgs::msg::PoseStamped& pose, Eigen::Matrix4d& T); + +void geometryPoseToEigen(const geometry_msgs::msg::PoseWithCovarianceStamped& odomLidar, Eigen::Matrix4d& T); + +void odomMsgToTf(const nav_msgs::msg::Odometry& odomLidar, tf2::Transform& T); + +tf2::Transform matrix3ToTf(const Eigen::Matrix3d& R); + +tf2::Transform matrix4ToTf(const Eigen::Matrix4d& T); + +tf2::Transform isometry3ToTf(const Eigen::Isometry3d& T); + +void tfToMatrix4(const tf2::Transform& tf_T, Eigen::Matrix4d& T); + +void tfToIsometry3(const tf2::Transform& tf_T, Eigen::Isometry3d& T); + +void transformStampedToIsometry3(const geometry_msgs::msg::Transform& transform, Eigen::Isometry3d& T); + +tf2::Transform pose3ToTf(const Eigen::Matrix3d& T); + +} // namespace graph_msf diff --git a/ros2/graph_msf_ros2/package.xml b/ros2/graph_msf_ros2/package.xml new file mode 100644 index 00000000..51cc492c --- /dev/null +++ b/ros2/graph_msf_ros2/package.xml @@ -0,0 +1,41 @@ + + + graph_msf_ros2 + 0.0.0 + + State estimation based on factor graphs, utilizing GTSAM functionality. Fusion of IMU, LiDAR Mapping Odometry + and GNSS estimates. + + + Julian Nubert + BSD + Julian Nubert + + + ament_cmake + rosidl_default_generators + + rclcpp + graph_msf + kdl_parser + nav_msgs + geometry_msgs + sensor_msgs + std_msgs + tf2 + tf2_ros + + urdf + eigen_colcon + tf2_eigen + graph_msf_ros2_msgs + + + python3 + + + ament_cmake_pytest + rosbag2_py + + rosidl_interface_packages + diff --git a/ros2/graph_msf_ros2/rviz/vis.rviz b/ros2/graph_msf_ros2/rviz/vis.rviz new file mode 100644 index 00000000..029f6208 --- /dev/null +++ b/ros2/graph_msf_ros2/rviz/vis.rviz @@ -0,0 +1,203 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Odometry1 + - /Odometry1/Shape1 + - /Odometry2 + - /Odometry2/Shape1 + Splitter Ratio: 0.5 + Tree Height: 1079 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Queue Size: 10 + Shape: + Alpha: 0.5 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.10000000149011612 + Head Radius: 0.10000000149011612 + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /crl_rzr/liopard/lio/odometry + Unreliable: false + Value: true + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 238; 238; 236 + Head Length: 0.10000000149011612 + Head Radius: 0.10000000149011612 + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /graph_msf/transform_odom_lidar + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: crl_rzr/map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Angle: -1.5699995756149292 + Class: rviz/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 94.30439758300781 + Target Frame: + X: 0 + Y: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1376 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000018a000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000074d0000003efc0100000002fb0000000800540069006d006501000000000000074d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004a8000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1869 + X: 691 + Y: 3 diff --git a/ros2/graph_msf_ros2/setup.py b/ros2/graph_msf_ros2/setup.py new file mode 100644 index 00000000..f0bc660f --- /dev/null +++ b/ros2/graph_msf_ros2/setup.py @@ -0,0 +1,8 @@ +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +setup_args = generate_distutils_setup( + package_dir={"": "python"}, +) + +setup(**setup_args) \ No newline at end of file diff --git a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp new file mode 100644 index 00000000..34b435d7 --- /dev/null +++ b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp @@ -0,0 +1,715 @@ +// Implementation +#include "graph_msf_ros2/GraphMsfRos2.h" + +// ROS2 +#include +#include + +// Workspace +#include "graph_msf_ros2/constants.h" +#include "graph_msf_ros2/util/conversions.h" + +namespace graph_msf { + +GraphMsfRos2::GraphMsfRos2(std::shared_ptr& node) : node_(node) { + RCLCPP_INFO(node_->get_logger(), "GraphMsfRos2-Constructor called."); + // Configurations ---------------------------- + // Graph Config + graphConfigPtr_ = std::make_shared(); +} + +void GraphMsfRos2::setup(std::shared_ptr staticTransformsPtr) { + RCLCPP_INFO(node_->get_logger(), "GraphMsfRos2-Setup called."); + + // Check + if (staticTransformsPtr == nullptr) { + throw std::runtime_error("Static transforms not set. Has to be set."); + } + + // Sensor Params + node_->declare_parameter("sensor_params.imuRate", 0.0); + node_->declare_parameter("sensor_params.createStateEveryNthImuMeasurement", 25); + node_->declare_parameter("sensor_params.useImuSignalLowPassFilter", false); + node_->declare_parameter("sensor_params.imuLowPassFilterCutoffFreq", 0.0); + node_->declare_parameter("sensor_params.imuBufferLength", 800); + node_->declare_parameter("sensor_params.imuTimeOffset", 0.0); + + // Initialization Params + node_->declare_parameter("initialization_params.estimateGravityFromImu", false); + node_->declare_parameter("initialization_params.gravityMagnitude", 9.80665); + + // Graph Params + node_->declare_parameter("graph_params.realTimeSmootherUseIsam", false); + node_->declare_parameter("graph_params.realTimeSmootherLag", 0.0); + node_->declare_parameter("graph_params.useAdditionalSlowBatchSmoother", false); + node_->declare_parameter("graph_params.slowBatchSmootherUseIsam", false); + node_->declare_parameter("graph_params.gaussNewtonWildfireThreshold", 0.0); + node_->declare_parameter("graph_params.minOptimizationFrequency", 0.0); + node_->declare_parameter("graph_params.maxOptimizationFrequency", 0.0); + node_->declare_parameter("graph_params.additionalOptimizationIterations", 0); + node_->declare_parameter("graph_params.findUnusedFactorSlots", false); + node_->declare_parameter("graph_params.enableDetailedResults", false); + node_->declare_parameter("graph_params.usingCholeskyFactorization", false); + node_->declare_parameter("graph_params.usingBiasForPreIntegration", false); + node_->declare_parameter("graph_params.optimizeReferenceFramePosesWrtWorld", false); + node_->declare_parameter("graph_params.optimizeExtrinsicSensorToSensorCorrectedOffset", false); + node_->declare_parameter("graph_params.referenceFramePosesResetThreshold", 0.0); + node_->declare_parameter("graph_params.centerReferenceFramesAtRobotPositionBeforeAlignment", false); + + // Noise Params + node_->declare_parameter("noise_params.accNoiseDensity", 0.0); + node_->declare_parameter("noise_params.integrationNoiseDensity", 0.0); + node_->declare_parameter("noise_params.use2ndOrderCoriolis", false); + node_->declare_parameter("noise_params.gyrNoiseDensity", 0.0); + node_->declare_parameter("noise_params.omegaCoriolis", 0.0); + node_->declare_parameter("noise_params.accBiasRandomWalkNoiseDensity", 0.0); + node_->declare_parameter("noise_params.gyrBiasRandomWalkNoiseDensity", 0.0); + node_->declare_parameter("noise_params.biasAccOmegaInit", 0.0); + node_->declare_parameter("noise_params.accBiasPrior", 0.0); + node_->declare_parameter("noise_params.gyrBiasPrior", 0.0); + node_->declare_parameter("noise_params.initialPositionNoiseDensity", 0.0); + node_->declare_parameter("noise_params.initialOrientationNoiseDensity", 0.0); + node_->declare_parameter("noise_params.initialVelocityNoiseDensity", 0.0); + node_->declare_parameter("noise_params.initialAccBiasNoiseDensity", 0.0); + node_->declare_parameter("noise_params.initialGyroBiasNoiseDensity", 0.0); + + // Re-linearization Params + node_->declare_parameter("relinearization_params.positionReLinTh", 0.0); + node_->declare_parameter("relinearization_params.rotationReLinTh", 0.0); + node_->declare_parameter("relinearization_params.velocityReLinTh", 0.0); + node_->declare_parameter("relinearization_params.accBiasReLinTh", 0.0); + node_->declare_parameter("relinearization_params.gyrBiasReLinTh", 0.0); + node_->declare_parameter("relinearization_params.referenceFrameReLinTh", 0.0); + node_->declare_parameter("relinearization_params.calibrationReLinTh", 0.0); + node_->declare_parameter("relinearization_params.displacementReLinTh", 0.0); + node_->declare_parameter("relinearization_params.landmarkReLinTh", 0.0); + node_->declare_parameter("relinearization_params.relinearizeSkip", 0); + node_->declare_parameter("relinearization_params.enableRelinearization", false); + node_->declare_parameter("relinearization_params.evaluateNonlinearError", false); + node_->declare_parameter("relinearization_params.cacheLinearizedFactors", false); + node_->declare_parameter("relinearization_params.enablePartialRelinearizationCheck", false); + + // Common Params + node_->declare_parameter("common_params.verbosity", 0); + node_->declare_parameter("common_params.odomNotJumpAtStart", false); + + // Extrinsic frames + node_->declare_parameter("extrinsics.worldFrame", std::string("")); + node_->declare_parameter("extrinsics.odomFrame", std::string("")); + node_->declare_parameter("extrinsics.imuFrame", std::string("")); + node_->declare_parameter("extrinsics.initializeZeroYawAndPositionOfFrame", std::string("")); + node_->declare_parameter("extrinsics.baseLinkFrame", std::string("")); + + // Name IDs + node_->declare_parameter("name_ids.referenceFrameAligned", std::string("")); + node_->declare_parameter("name_ids.sensorFrameCorrected", std::string("")); + + // Logging path + node_->declare_parameter("launch.optimizationResultLoggingPath", std::string("")); + + // Read parameters ---------------------------- + GraphMsfRos2::readParams(); + + // Call super class Setup ---------------------------- + GraphMsf::setup(graphConfigPtr_, staticTransformsPtr); + + // Publishers ---------------------------- + GraphMsfRos2::initializePublishers(); + + // Subscribers ---------------------------- + GraphMsfRos2::initializeSubscribers(); + + // Messages ---------------------------- + GraphMsfRos2::initializeMessages(); + + tfBroadcaster_ = std::make_shared(node_); + + // Services ---------------------------- + // GraphMsfRos::initializeServices(*node_); + + // Time + startTime = std::chrono::high_resolution_clock::now(); + + // Wrap up + RCLCPP_INFO(node_->get_logger(), "Set up successfully."); +} + +void GraphMsfRos2::initializePublishers() { + RCLCPP_INFO(node_->get_logger(), "Initializing publishers."); + + // Odometry + pubEstOdomImu_ = node_->create_publisher("/graph_msf/est_odometry_odom_imu", ROS_QUEUE_SIZE); + pubEstWorldImu_ = + node_->create_publisher("/graph_msf/est_odometry_world_imu", ROS_QUEUE_SIZE); + pubOptWorldImu_ = + node_->create_publisher("/graph_msf/opt_odometry_world_imu", ROS_QUEUE_SIZE); + + // Vector3 Variances + pubEstWorldPosVariance_ = + node_->create_publisher("/graph_msf/est_world_pos_variance", ROS_QUEUE_SIZE); + pubEstWorldRotVariance_ = + node_->create_publisher("/graph_msf/est_world_rot_variance", ROS_QUEUE_SIZE); + + // Velocity Marker + pubVelocityMarker_ = + node_->create_publisher("/graph_msf/velocity_marker", ROS_QUEUE_SIZE); + + // Paths + pubEstOdomImuPath_ = node_->create_publisher("/graph_msf/est_path_odom_imu", ROS_QUEUE_SIZE); + pubEstWorldImuPath_ = node_->create_publisher("/graph_msf/est_path_world_imu", ROS_QUEUE_SIZE); + pubOptWorldImuPath_ = node_->create_publisher("/graph_msf/opt_path_world_imu", ROS_QUEUE_SIZE); + + // Imu Bias + pubAccelBias_ = node_->create_publisher("/graph_msf/accel_bias", ROS_QUEUE_SIZE); + pubGyroBias_ = node_->create_publisher("/graph_msf/gyro_bias", ROS_QUEUE_SIZE); + + // Added Imu Measurements + pubAddedImuMeas_ = node_->create_publisher("/graph_msf/added_imu_meas", ROS_QUEUE_SIZE); +} + +void GraphMsfRos2::initializeSubscribers() { + RCLCPP_INFO(node_->get_logger(), "Initializing subscribers."); + + // Imu + subImu_ = node_->create_subscription( + "/imu_broadcaster/imu_bike_base_front_top", rclcpp::QoS(ROS_QUEUE_SIZE).best_effort(), + std::bind(&GraphMsfRos2::imuCallback, this, std::placeholders::_1)); + + RCLCPP_INFO(node_->get_logger(), "GraphMsfRos2 Initialized main IMU subscriber with topic: %s", + subImu_->get_topic_name()); +} + +void GraphMsfRos2::initializeMessages() { + RCLCPP_INFO(node_->get_logger(), "Initializing messages."); + + // Odometry + estOdomImuMsgPtr_ = std::make_shared(); + estWorldImuMsgPtr_ = std::make_shared(); + optWorldImuMsgPtr_ = std::make_shared(); + + // Vector3 Variances + estWorldPosVarianceMsgPtr_ = std::make_shared(); + estWorldRotVarianceMsgPtr_ = std::make_shared(); + + // Path + estOdomImuPathPtr_ = std::make_shared(); + estWorldImuPathPtr_ = std::make_shared(); + optWorldImuPathPtr_ = std::make_shared(); + + // Imu Bias + accelBiasMsgPtr_ = std::make_shared(); + gyroBiasMsgPtr_ = std::make_shared(); +} + +// void GraphMsfRos2::initializeServices(rclcpp::Node& node) { +// RCLCPP_INFO(node.get_logger(), "Initializing services."); + +// // Trigger offline smoother optimization +// srvSmootherOptimize_ = node.create_service( +// "/graph_msf/trigger_offline_optimization", +// std::bind(&GraphMsfRos::srvOfflineSmootherOptimizeCallback, this, std::placeholders::_1, +// std::placeholders::_2)); +// } + +// bool GraphMsfRos2::srvOfflineSmootherOptimizeCallback( +// const std::shared_ptr req, +// std::shared_ptr res) { +// // Max Iterations from service call +// int maxIterations = req->max_optimization_iterations; + +// // Trigger offline smoother optimization and create response +// if (GraphMsf::optimizeSlowBatchSmoother(maxIterations, optimizationResultLoggingPath)) { +// res->success = true; +// res->message = "Optimization successful."; +// } else { +// res->success = false; +// res->message = "Optimization failed."; +// } +// return true; +// } + +void GraphMsfRos2::addToPathMsg(const nav_msgs::msg::Path::SharedPtr& pathPtr, const std::string& fixedFrameName, + const rclcpp::Time& stamp, const Eigen::Vector3d& t, const int maxBufferLength) { + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = fixedFrameName; + pose.header.stamp = stamp; + pose.pose.position.x = t(0); + pose.pose.position.y = t(1); + pose.pose.position.z = t(2); + pathPtr->header.frame_id = fixedFrameName; + pathPtr->header.stamp = stamp; + pathPtr->poses.push_back(pose); + if (pathPtr->poses.size() > maxBufferLength) { + pathPtr->poses.erase(pathPtr->poses.begin()); + } +} + +void GraphMsfRos2::addToOdometryMsg(const nav_msgs::msg::Odometry::SharedPtr& msgPtr, const std::string& fixedFrame, + const std::string& movingFrame, const rclcpp::Time& stamp, + const Eigen::Isometry3d& T, const Eigen::Vector3d& F_v_W_F, + const Eigen::Vector3d& F_w_W_F, const Eigen::Matrix& poseCovariance, + const Eigen::Matrix& twistCovariance) { + msgPtr->header.frame_id = fixedFrame; + msgPtr->child_frame_id = movingFrame; + msgPtr->header.stamp = stamp; + + // Convert Eigen::Isometry3d to geometry_msgs::msg::Pose using tf2 + geometry_msgs::msg::Pose pose_msg; + tf2::convert(T, pose_msg); + msgPtr->pose.pose = pose_msg; + + msgPtr->twist.twist.linear.x = F_v_W_F(0); + msgPtr->twist.twist.linear.y = F_v_W_F(1); + msgPtr->twist.twist.linear.z = F_v_W_F(2); + msgPtr->twist.twist.angular.x = F_w_W_F(0); + msgPtr->twist.twist.angular.y = F_w_W_F(1); + msgPtr->twist.twist.angular.z = F_w_W_F(2); + + for (int i = 0; i < 6; i++) { + for (int j = 0; j < 6; j++) { + msgPtr->pose.covariance[6 * i + j] = poseCovariance(i, j); + msgPtr->twist.covariance[6 * i + j] = twistCovariance(i, j); + } + } +} + +void GraphMsfRos2::addToPoseWithCovarianceStampedMsg( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr& msgPtr, const std::string& frameName, + const rclcpp::Time& stamp, const Eigen::Isometry3d& T, const Eigen::Matrix& transformCovariance) { + msgPtr->header.frame_id = frameName; + msgPtr->header.stamp = stamp; + + // Convert Eigen::Isometry3d to geometry_msgs::msg::Pose using tf2 + geometry_msgs::msg::Pose pose_msg; + tf2::convert(T, pose_msg); + msgPtr->pose.pose = pose_msg; + + for (int i = 0; i < 6; i++) { + for (int j = 0; j < 6; j++) { + msgPtr->pose.covariance[6 * i + j] = transformCovariance(i, j); + } + } +} + +void GraphMsfRos2::extractCovariancesFromOptimizedState( + Eigen::Matrix& poseCovarianceRos, Eigen::Matrix& twistCovarianceRos, + const std::shared_ptr& optimizedStateWithCovarianceAndBiasPtr) { + // Extract covariances from optimized state + if (optimizedStateWithCovarianceAndBiasPtr != nullptr) { + poseCovarianceRos = graph_msf::convertCovarianceGtsamConventionToRosConvention( + optimizedStateWithCovarianceAndBiasPtr->getPoseCovariance()); + twistCovarianceRos.block<3, 3>(0, 0) = optimizedStateWithCovarianceAndBiasPtr->getVelocityCovariance(); + } else { + poseCovarianceRos.setZero(); + twistCovarianceRos.setZero(); + } +} + +// Markers +void GraphMsfRos2::createVelocityMarker(const std::string& referenceFrameName, const rclcpp::Time& stamp, + const Eigen::Vector3d& velocity, visualization_msgs::msg::Marker& marker) { + // Arrow + marker.header.frame_id = referenceFrameName; + marker.header.stamp = stamp; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = visualization_msgs::msg::Marker::ADD; + + // Scale and Color + const double scale = velocity.norm(); + marker.scale.x = 0.1; // shaft diameter + marker.scale.y = 0.2; // head diameter + marker.scale.z = 0.2; // head length + marker.color.a = 1.0; + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + + // Define Arrow through start and end point + geometry_msgs::msg::Point startPoint, endPoint; + startPoint.x = 0.0; // origin + startPoint.y = 0.0; // origin + startPoint.z = 1.0; // 1 meter above origin + endPoint.x = startPoint.x + velocity(0); + endPoint.y = startPoint.y + velocity(1); + endPoint.z = startPoint.z + velocity(2); + marker.points.push_back(startPoint); + marker.points.push_back(endPoint); + + // Quaternion for orientation + tf2::Quaternion q; + q.setRPY(0, 0, 0); + marker.pose.orientation.x = q.x(); + marker.pose.orientation.y = q.y(); + marker.pose.orientation.z = q.z(); + marker.pose.orientation.w = q.w(); +} + +void GraphMsfRos2::createContactMarker(const std::string& referenceFrameName, const rclcpp::Time& stamp, + const Eigen::Vector3d& position, const std::string& nameSpace, const int id, + visualization_msgs::msg::Marker& marker) { + // Sphere + marker.header.frame_id = referenceFrameName; + marker.header.stamp = stamp; + marker.ns = nameSpace; + marker.id = id; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + + // Scale and Color + marker.scale.x = 0.1; + marker.scale.y = 0.1; + marker.scale.z = 0.1; + marker.color.a = 1.0; + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + + // Position + marker.pose.position.x = position(0); + marker.pose.position.y = position(1); + marker.pose.position.z = position(2); + + // Orientation + marker.pose.orientation.w = 1.0; +} + +long GraphMsfRos2::secondsSinceStart() { + currentTime = std::chrono::high_resolution_clock::now(); + return std::chrono::duration_cast(currentTime - startTime).count(); +} + +// Main IMU Callback +void GraphMsfRos2::imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuMsgPtr) { + // Convert to Eigen + Eigen::Vector3d linearAcc(imuMsgPtr->linear_acceleration.x, imuMsgPtr->linear_acceleration.y, + imuMsgPtr->linear_acceleration.z); + Eigen::Vector3d angularVel(imuMsgPtr->angular_velocity.x, imuMsgPtr->angular_velocity.y, + imuMsgPtr->angular_velocity.z); + Eigen::Matrix addedImuMeasurements; // accel, gyro + + // Create pointer for carrying state + std::shared_ptr preIntegratedNavStatePtr = nullptr; + std::shared_ptr optimizedStateWithCovarianceAndBiasPtr = nullptr; + + // Add measurement and get state + if (GraphMsf::addCoreImuMeasurementAndGetState( + linearAcc, angularVel, imuMsgPtr->header.stamp.sec + 1e-9 * imuMsgPtr->header.stamp.nanosec, + preIntegratedNavStatePtr, optimizedStateWithCovarianceAndBiasPtr, addedImuMeasurements)) { + // Encountered Delay + auto now = node_->now(); + auto delay = now.seconds() - preIntegratedNavStatePtr->getTimeK(); + if (delay > 0.5) { + RCLCPP_WARN(node_->get_logger(), "Encountered delay of %.14f seconds.", delay); + } + + // Publish Odometry + this->publishState(preIntegratedNavStatePtr, optimizedStateWithCovarianceAndBiasPtr); + + // Publish Filtered Imu Measurements (uncomment if needed) + // this->publishAddedImuMeas_(addedImuMeasurements, imuMsgPtr->header.stamp); + } else if (GraphMsf::isGraphInited()) { + RCLCPP_WARN(node_->get_logger(), "Could not add IMU measurement."); + } +} + +// Publish state --------------------------------------------------------------- +// Higher Level Functions +void GraphMsfRos2::publishState( + const std::shared_ptr& integratedNavStatePtr, + const std::shared_ptr& optimizedStateWithCovarianceAndBiasPtr) { + // Covariances + Eigen::Matrix poseCovarianceRos, twistCovarianceRos; + graph_msf::GraphMsfRos2::extractCovariancesFromOptimizedState(poseCovarianceRos, twistCovarianceRos, + optimizedStateWithCovarianceAndBiasPtr); + + // Variances (only diagonal elements) + Eigen::Vector3d positionVarianceRos = poseCovarianceRos.block<3, 3>(0, 0).diagonal(); + Eigen::Vector3d orientationVarianceRos = poseCovarianceRos.block<3, 3>(3, 3).diagonal(); + + // Publish non-time critical data in a separate thread + std::thread publishNonTimeCriticalDataThread(&GraphMsfRos2::publishNonTimeCriticalData, this, poseCovarianceRos, + twistCovarianceRos, positionVarianceRos, orientationVarianceRos, + integratedNavStatePtr, optimizedStateWithCovarianceAndBiasPtr); + publishNonTimeCriticalDataThread.detach(); +} + +// Copy the arguments in order to be thread safe +void GraphMsfRos2::publishNonTimeCriticalData( + const Eigen::Matrix poseCovarianceRos, const Eigen::Matrix twistCovarianceRos, + const Eigen::Vector3d positionVarianceRos, const Eigen::Vector3d orientationVarianceRos, + const std::shared_ptr integratedNavStatePtr, + const std::shared_ptr optimizedStateWithCovarianceAndBiasPtr) { + // Mutex for not overloading ROS + std::lock_guard lock(rosPublisherMutex_); + + // Time + const double& timeK = integratedNavStatePtr->getTimeK(); // Alias + + // Odometry messages + publishImuOdoms(integratedNavStatePtr, poseCovarianceRos, twistCovarianceRos); + + // Publish to TF + // B_O + Eigen::Isometry3d T_B_Ok = staticTransformsPtr_->rv_T_frame1_frame2(staticTransformsPtr_->getBaseLinkFrame(), + staticTransformsPtr_->getImuFrame()) * + integratedNavStatePtr->getT_O_Ik_gravityAligned().inverse(); + publishTfTreeTransform(staticTransformsPtr_->getBaseLinkFrame(), staticTransformsPtr_->getOdomFrame(), timeK, T_B_Ok); + // O_W + Eigen::Isometry3d T_O_W = integratedNavStatePtr->getT_W_O().inverse(); + publishTfTreeTransform(staticTransformsPtr_->getOdomFrame(), staticTransformsPtr_->getWorldFrame(), timeK, T_O_W); + + // Publish Variances + publishDiagVarianceVectors(positionVarianceRos, orientationVarianceRos, timeK); + + // Publish Velocity Markers + publishVelocityMarkers(integratedNavStatePtr); + + // Publish paths + publishImuPaths(integratedNavStatePtr); + + // Optimized estimate ---------------------- + publishOptimizedStateAndBias(optimizedStateWithCovarianceAndBiasPtr, poseCovarianceRos, twistCovarianceRos); +} + +void GraphMsfRos2::publishOptimizedStateAndBias( + const std::shared_ptr optimizedStateWithCovarianceAndBiasPtr, + const Eigen::Matrix& poseCovarianceRos, const Eigen::Matrix& twistCovarianceRos) { + if (optimizedStateWithCovarianceAndBiasPtr != nullptr && + optimizedStateWithCovarianceAndBiasPtr->getTimeK() - lastOptimizedStateTimestamp_ > 1e-03) { + // Time of this optimized state + lastOptimizedStateTimestamp_ = optimizedStateWithCovarianceAndBiasPtr->getTimeK(); + + // Odometry messages + // world->imu + if (pubOptWorldImu_->get_subscription_count() > 0) { + addToOdometryMsg(optWorldImuMsgPtr_, staticTransformsPtr_->getWorldFrame(), staticTransformsPtr_->getImuFrame(), + rclcpp::Time(optimizedStateWithCovarianceAndBiasPtr->getTimeK() * 1e9), + optimizedStateWithCovarianceAndBiasPtr->getT_W_Ik(), + optimizedStateWithCovarianceAndBiasPtr->getI_v_W_I(), + optimizedStateWithCovarianceAndBiasPtr->getI_w_W_I(), poseCovarianceRos, twistCovarianceRos); + pubOptWorldImu_->publish(*optWorldImuMsgPtr_); + } + + // Path + // world->imu + addToPathMsg(optWorldImuPathPtr_, staticTransformsPtr_->getWorldFrame(), + rclcpp::Time(optimizedStateWithCovarianceAndBiasPtr->getTimeK() * 1e9), + optimizedStateWithCovarianceAndBiasPtr->getT_W_Ik().translation(), + graphConfigPtr_->imuBufferLength_ * 20); + if (pubOptWorldImuPath_->get_subscription_count() > 0) { + pubOptWorldImuPath_->publish(*optWorldImuPathPtr_); + } + + // Biases + // Publish accel bias + if (pubAccelBias_->get_subscription_count() > 0) { + Eigen::Vector3d accelBias = optimizedStateWithCovarianceAndBiasPtr->getAccelerometerBias(); + accelBiasMsgPtr_->header.stamp = rclcpp::Time(optimizedStateWithCovarianceAndBiasPtr->getTimeK() * 1e9); + accelBiasMsgPtr_->header.frame_id = staticTransformsPtr_->getImuFrame(); + accelBiasMsgPtr_->vector.x = accelBias(0); + accelBiasMsgPtr_->vector.y = accelBias(1); + accelBiasMsgPtr_->vector.z = accelBias(2); + pubAccelBias_->publish(*accelBiasMsgPtr_); + } + // Publish gyro bias + if (pubGyroBias_->get_subscription_count() > 0) { + Eigen::Vector3d gyroBias = optimizedStateWithCovarianceAndBiasPtr->getGyroscopeBias(); + gyroBiasMsgPtr_->header.stamp = rclcpp::Time(optimizedStateWithCovarianceAndBiasPtr->getTimeK() * 1e9); + gyroBiasMsgPtr_->header.frame_id = staticTransformsPtr_->getImuFrame(); + gyroBiasMsgPtr_->vector.x = gyroBias(0); + gyroBiasMsgPtr_->vector.y = gyroBias(1); + gyroBiasMsgPtr_->vector.z = gyroBias(2); + pubGyroBias_->publish(*gyroBiasMsgPtr_); + } + + // TFs in Optimized State + for (const auto& framePairTransformMapIterator : + optimizedStateWithCovarianceAndBiasPtr->getReferenceFrameTransforms().getTransformsMap()) { + // Case 1: If includes world frame --> everything child of world -------------------------------- + if (framePairTransformMapIterator.first.first == staticTransformsPtr_->getWorldFrame() || + framePairTransformMapIterator.first.second == staticTransformsPtr_->getWorldFrame()) { + // A. Get transform + Eigen::Isometry3d T_W_M; + std::string mapFrameName; + const std::string& worldFrameName = staticTransformsPtr_->getWorldFrame(); + + // If world is first, then map is second + if (framePairTransformMapIterator.first.first == worldFrameName) { + T_W_M = framePairTransformMapIterator.second.inverse(); + mapFrameName = framePairTransformMapIterator.first.second; + } + // If world is second, then map is first, this is the case for holistic alignment + else { + T_W_M = framePairTransformMapIterator.second; + mapFrameName = framePairTransformMapIterator.first.first; + + // B. Publish TransformStamped for Aligned Frames + std::string transformTopic = + "/graph_msf/transform_" + worldFrameName + "_to_" + mapFrameName + referenceFrameAlignedNameId; + auto poseWithCovarianceStampedMsgPtr = std::make_shared(); + addToPoseWithCovarianceStampedMsg( + poseWithCovarianceStampedMsgPtr, staticTransformsPtr_->getWorldFrame(), + rclcpp::Time(optimizedStateWithCovarianceAndBiasPtr->getTimeK() * 1e9), T_W_M, + optimizedStateWithCovarianceAndBiasPtr->getReferenceFrameTransformsCovariance().rv_T_frame1_frame2( + framePairTransformMapIterator.first.first, framePairTransformMapIterator.first.second)); + // Check whether publisher already exists + if (pubPoseStampedByTopicMap_.find(transformTopic) == pubPoseStampedByTopicMap_.end()) { + pubPoseStampedByTopicMap_[transformTopic] = + node_->create_publisher(transformTopic, 1); + RCLCPP_INFO(node_->get_logger(), "Initialized publisher for %s", transformTopic.c_str()); + } + if (pubPoseStampedByTopicMap_[transformTopic]->get_subscription_count() > 0) { + pubPoseStampedByTopicMap_[transformTopic]->publish(*poseWithCovarianceStampedMsgPtr); + } + } + + // C. Publish TF Tree + publishTfTreeTransform(worldFrameName, mapFrameName + referenceFrameAlignedNameId, + optimizedStateWithCovarianceAndBiasPtr->getTimeK(), T_W_M); + + } + // Case 2: Other transformations + else { + const std::string& sensorFrameName = framePairTransformMapIterator.first.first; + const std::string& sensorFrameNameCorrected = framePairTransformMapIterator.first.second; + const Eigen::Isometry3d& T_sensor_sensorCorrected = framePairTransformMapIterator.second; + + // A. Publish TransformStamped for Corrected Frames + std::string transformTopic = "/graph_msf/transform_" + sensorFrameName + "_to_" + sensorFrameNameCorrected; + auto poseWithCovarianceStampedMsgPtr = std::make_shared(); + addToPoseWithCovarianceStampedMsg( + poseWithCovarianceStampedMsgPtr, sensorFrameName, + rclcpp::Time(optimizedStateWithCovarianceAndBiasPtr->getTimeK() * 1e9), T_sensor_sensorCorrected, + optimizedStateWithCovarianceAndBiasPtr->getReferenceFrameTransformsCovariance().rv_T_frame1_frame2( + sensorFrameName, sensorFrameNameCorrected)); + // Check whether publisher already exists + if (pubPoseStampedByTopicMap_.find(transformTopic) == pubPoseStampedByTopicMap_.end()) { + pubPoseStampedByTopicMap_[transformTopic] = + node_->create_publisher(transformTopic, 1); + RCLCPP_INFO(node_->get_logger(), "Initialized publisher for %s", transformTopic.c_str()); + } + if (pubPoseStampedByTopicMap_[transformTopic]->get_subscription_count() > 0) { + pubPoseStampedByTopicMap_[transformTopic]->publish(*poseWithCovarianceStampedMsgPtr); + } + + // B. Publish TF Tree + publishTfTreeTransform(sensorFrameName, sensorFrameNameCorrected, + optimizedStateWithCovarianceAndBiasPtr->getTimeK(), T_sensor_sensorCorrected); + } + } + } +} + +// Lower Level Functions +void GraphMsfRos2::publishTfTreeTransform(const std::string& parentFrameName, const std::string& childFrameName, + const double timeStamp, const Eigen::Isometry3d& T_frame_childFrame) { + geometry_msgs::msg::TransformStamped transformStamped; + transformStamped.header.stamp = rclcpp::Time(timeStamp * 1e9); + transformStamped.header.frame_id = parentFrameName; + transformStamped.child_frame_id = childFrameName; + transformStamped.transform.translation.x = T_frame_childFrame.translation().x(); + transformStamped.transform.translation.y = T_frame_childFrame.translation().y(); + transformStamped.transform.translation.z = T_frame_childFrame.translation().z(); + Eigen::Quaterniond q(T_frame_childFrame.rotation()); + transformStamped.transform.rotation.x = q.x(); + transformStamped.transform.rotation.y = q.y(); + transformStamped.transform.rotation.z = q.z(); + transformStamped.transform.rotation.w = q.w(); + + // Convert Eigen::Isometry3d to geometry_msgs::msg::Transform + // tf2::convert(T_frame_childFrame, transformStamped.transform); + + tfBroadcaster_->sendTransform(transformStamped); +} + +void GraphMsfRos2::publishImuOdoms( + const std::shared_ptr& preIntegratedNavStatePtr, + const Eigen::Matrix& poseCovarianceRos, const Eigen::Matrix& twistCovarianceRos) const { + // Odom->imu + if (pubEstOdomImu_->get_subscription_count() > 0) { + addToOdometryMsg(estOdomImuMsgPtr_, staticTransformsPtr_->getOdomFrame(), staticTransformsPtr_->getImuFrame(), + rclcpp::Time(preIntegratedNavStatePtr->getTimeK() * 1e9), + preIntegratedNavStatePtr->getT_O_Ik_gravityAligned(), preIntegratedNavStatePtr->getI_v_W_I(), + preIntegratedNavStatePtr->getI_w_W_I(), poseCovarianceRos, twistCovarianceRos); + pubEstOdomImu_->publish(*estOdomImuMsgPtr_); + } + // World->imu + if (pubEstWorldImu_->get_subscription_count() > 0) { + addToOdometryMsg(estWorldImuMsgPtr_, staticTransformsPtr_->getWorldFrame(), staticTransformsPtr_->getImuFrame(), + rclcpp::Time(preIntegratedNavStatePtr->getTimeK() * 1e9), preIntegratedNavStatePtr->getT_W_Ik(), + preIntegratedNavStatePtr->getI_v_W_I(), preIntegratedNavStatePtr->getI_w_W_I(), poseCovarianceRos, + twistCovarianceRos); + pubEstWorldImu_->publish(*estWorldImuMsgPtr_); + } +} + +void GraphMsfRos2::publishDiagVarianceVectors(const Eigen::Vector3d& posVarianceRos, + const Eigen::Vector3d& rotVarianceRos, const double timeStamp) const { + // World Position Variance + if (pubEstWorldPosVariance_->get_subscription_count() > 0) { + estWorldPosVarianceMsgPtr_->header.stamp = rclcpp::Time(timeStamp * 1e9); + estWorldPosVarianceMsgPtr_->header.frame_id = staticTransformsPtr_->getWorldFrame(); + estWorldPosVarianceMsgPtr_->vector.x = posVarianceRos(0); + estWorldPosVarianceMsgPtr_->vector.y = posVarianceRos(1); + estWorldPosVarianceMsgPtr_->vector.z = posVarianceRos(2); + pubEstWorldPosVariance_->publish(*estWorldPosVarianceMsgPtr_); + } + // World Rotation Variance + if (pubEstWorldRotVariance_->get_subscription_count() > 0) { + estWorldRotVarianceMsgPtr_->header.stamp = rclcpp::Time(timeStamp * 1e9); + estWorldRotVarianceMsgPtr_->header.frame_id = staticTransformsPtr_->getWorldFrame(); + estWorldRotVarianceMsgPtr_->vector.x = rotVarianceRos(0); + estWorldRotVarianceMsgPtr_->vector.y = rotVarianceRos(1); + estWorldRotVarianceMsgPtr_->vector.z = rotVarianceRos(2); + pubEstWorldRotVariance_->publish(*estWorldRotVarianceMsgPtr_); + } +} + +void GraphMsfRos2::publishVelocityMarkers( + const std::shared_ptr& navStatePtr) const { + // Velocity in Odom Frame Marker + visualization_msgs::msg::Marker velocityMarker; + createVelocityMarker(staticTransformsPtr_->getImuFrame(), rclcpp::Time(navStatePtr->getTimeK() * 1e9), + navStatePtr->getI_v_W_I(), velocityMarker); + + // Publish + if (pubVelocityMarker_->get_subscription_count() > 0) { + pubVelocityMarker_->publish(velocityMarker); + } +} + +void GraphMsfRos2::publishImuPaths(const std::shared_ptr& navStatePtr) const { + // odom->imu + addToPathMsg(estOdomImuPathPtr_, staticTransformsPtr_->getOdomFrame(), rclcpp::Time(navStatePtr->getTimeK() * 1e9), + navStatePtr->getT_O_Ik_gravityAligned().translation(), graphConfigPtr_->imuBufferLength_ * 20); + if (pubEstOdomImuPath_->get_subscription_count() > 0) { + pubEstOdomImuPath_->publish(*estOdomImuPathPtr_); + } + // world->imu + addToPathMsg(estWorldImuPathPtr_, staticTransformsPtr_->getWorldFrame(), rclcpp::Time(navStatePtr->getTimeK() * 1e9), + navStatePtr->getT_W_Ik().translation(), graphConfigPtr_->imuBufferLength_ * 20); + if (pubEstWorldImuPath_->get_subscription_count() > 0) { + pubEstWorldImuPath_->publish(*estWorldImuPathPtr_); + } +} + +void GraphMsfRos2::publishAddedImuMeas(const Eigen::Matrix& addedImuMeas, + const rclcpp::Time& stamp) const { + // Publish added imu measurement + if (pubAddedImuMeas_->get_subscription_count() > 0) { + sensor_msgs::msg::Imu addedImuMeasMsg; + addedImuMeasMsg.header.stamp = stamp; + addedImuMeasMsg.header.frame_id = staticTransformsPtr_->getImuFrame(); + addedImuMeasMsg.linear_acceleration.x = addedImuMeas(0); + addedImuMeasMsg.linear_acceleration.y = addedImuMeas(1); + addedImuMeasMsg.linear_acceleration.z = addedImuMeas(2); + addedImuMeasMsg.angular_velocity.x = addedImuMeas(3); + addedImuMeasMsg.angular_velocity.y = addedImuMeas(4); + addedImuMeasMsg.angular_velocity.z = addedImuMeas(5); + pubAddedImuMeas_->publish(addedImuMeasMsg); + } +} + +} // namespace graph_msf + diff --git a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2bu.cpp b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2bu.cpp new file mode 100644 index 00000000..0c3b0093 --- /dev/null +++ b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2bu.cpp @@ -0,0 +1,167 @@ +// Implementation +#include "graph_msf_ros2/GraphMsfRos2.h" + +// ROS 2 +#include +#include + +// Workspace +#include "graph_msf_ros2/constants.h" +#include "graph_msf_ros2/util/conversions.h" + +namespace graph_msf { + +GraphMsfRos2::GraphMsfRos2(const std::shared_ptr& nodePtr) : node_(nodePtr), tfBroadcaster(nodePtr) { + RCLCPP_INFO(node_->get_logger(), "GraphMsfRos2-Constructor called."); + graphConfigPtr_ = std::make_shared(); +} + +void GraphMsfRos2::setup(const std::shared_ptr& staticTransformsPtr) { + RCLCPP_INFO(node_->get_logger(), "GraphMsfRos2-Setup called."); + + if (staticTransformsPtr == nullptr) { + throw std::runtime_error("Static transforms not set. They must be set."); + } + + GraphMsfRos2::readParams(*node_); + GraphMsf::setup(graphConfigPtr_, staticTransformsPtr); + + initializePublishers(); + initializeSubscribers(); + initializeMessages(); + // initializeServices(); + + startTime = std::chrono::high_resolution_clock::now(); + RCLCPP_INFO(node_->get_logger(), "Set up successfully."); +} + +void GraphMsfRos2::initializePublishers() { + RCLCPP_INFO(node_->get_logger(), "Initializing publishers."); + + pubEstOdomImu_ = node_->create_publisher("/graph_msf/est_odometry_odom_imu", ROS_QUEUE_SIZE); + pubEstWorldImu_ = + node_->create_publisher("/graph_msf/est_odometry_world_imu", ROS_QUEUE_SIZE); + pubOptWorldImu_ = + node_->create_publisher("/graph_msf/opt_odometry_world_imu", ROS_QUEUE_SIZE); + + pubEstWorldPosVariance_ = + node_->create_publisher("/graph_msf/est_world_pos_variance", ROS_QUEUE_SIZE); + pubEstWorldRotVariance_ = + node_->create_publisher("/graph_msf/est_world_rot_variance", ROS_QUEUE_SIZE); + + pubVelocityMarker_ = + node_->create_publisher("/graph_msf/velocity_marker", ROS_QUEUE_SIZE); + pubEstOdomImuPath_ = node_->create_publisher("/graph_msf/est_path_odom_imu", ROS_QUEUE_SIZE); + pubEstWorldImuPath_ = node_->create_publisher("/graph_msf/est_path_world_imu", ROS_QUEUE_SIZE); + pubOptWorldImuPath_ = node_->create_publisher("/graph_msf/opt_path_world_imu", ROS_QUEUE_SIZE); + + pubAccelBias_ = node_->create_publisher("/graph_msf/accel_bias", ROS_QUEUE_SIZE); + pubGyroBias_ = node_->create_publisher("/graph_msf/gyro_bias", ROS_QUEUE_SIZE); + pubAddedImuMeas_ = node_->create_publisher("/graph_msf/added_imu_meas", ROS_QUEUE_SIZE); +} + +void GraphMsfRos2::initializeSubscribers() { + RCLCPP_INFO(node_->get_logger(), "Initializing subscribers."); + + subImu_ = node_->create_subscription( + "/imu_topic", ROS_QUEUE_SIZE, std::bind(&GraphMsfRos2::imuCallback, this, std::placeholders::_1)); + RCLCPP_INFO(node_->get_logger(), "Initialized main IMU subscriber with topic: /imu_topic"); +} + +void GraphMsfRos2::initializeMessages() { + RCLCPP_INFO(node_->get_logger(), "Initializing messages."); + + estOdomImuMsgPtr_ = std::make_shared(); + estWorldImuMsgPtr_ = std::make_shared(); + optWorldImuMsgPtr_ = std::make_shared(); + + estWorldPosVarianceMsgPtr_ = std::make_shared(); + estWorldRotVarianceMsgPtr_ = std::make_shared(); + + estOdomImuPathPtr_ = std::make_shared(); + estWorldImuPathPtr_ = std::make_shared(); + optWorldImuPathPtr_ = std::make_shared(); + + accelBiasMsgPtr_ = std::make_shared(); + gyroBiasMsgPtr_ = std::make_shared(); +} + +// void GraphMsfRos2::initializeServices() { +// srvSmootherOptimize_ = node_->create_service( +// "/graph_msf/trigger_offline_optimization", +// std::bind(&GraphMsfRos2::srvOfflineSmootherOptimizeCallback, this, std::placeholders::_1, +// std::placeholders::_2)); +// } + +// bool GraphMsfRos2::srvOfflineSmootherOptimizeCallback( +// const std::shared_ptr req, +// std::shared_ptr res) { +// int maxIterations = req->max_optimization_iterations; + +// if (GraphMsf::optimizeSlowBatchSmoother(maxIterations, optimizationResultLoggingPath)) { +// res->success = true; +// res->message = "Optimization successful."; +// } else { +// res->success = false; +// res->message = "Optimization failed."; +// } +// return true; +// } + +// Main IMU Callback +void GraphMsfRos2::imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuMsgPtr) { + Eigen::Vector3d linearAcc(imuMsgPtr->linear_acceleration.x, imuMsgPtr->linear_acceleration.y, + imuMsgPtr->linear_acceleration.z); + Eigen::Vector3d angularVel(imuMsgPtr->angular_velocity.x, imuMsgPtr->angular_velocity.y, + imuMsgPtr->angular_velocity.z); + Eigen::Matrix addedImuMeasurements; + + std::shared_ptr preIntegratedNavStatePtr = nullptr; + std::shared_ptr optimizedStateWithCovarianceAndBiasPtr = nullptr; + + if (GraphMsf::addCoreImuMeasurementAndGetState( + linearAcc, angularVel, imuMsgPtr->header.stamp.sec + 1e-9 * imuMsgPtr->header.stamp.nanosec, + preIntegratedNavStatePtr, optimizedStateWithCovarianceAndBiasPtr, addedImuMeasurements)) { + if (node_->now().seconds() - preIntegratedNavStatePtr->getTimeK() > 0.5) { + RCLCPP_WARN(node_->get_logger(), "Encountered delay of %f seconds.", + node_->now().seconds() - preIntegratedNavStatePtr->getTimeK()); + } + + this->publishState(preIntegratedNavStatePtr, optimizedStateWithCovarianceAndBiasPtr); + } else if (GraphMsf::isGraphInited()) { + RCLCPP_WARN(node_->get_logger(), "Could not add IMU measurement."); + } +} + +void GraphMsfRos2::publishState( + const std::shared_ptr& integratedNavStatePtr, + const std::shared_ptr& optimizedStateWithCovarianceAndBiasPtr) { + Eigen::Matrix poseCovarianceRos, twistCovarianceRos; + extractCovariancesFromOptimizedState(poseCovarianceRos, twistCovarianceRos, optimizedStateWithCovarianceAndBiasPtr); + + Eigen::Vector3d positionVarianceRos = poseCovarianceRos.block<3, 3>(0, 0).diagonal(); + Eigen::Vector3d orientationVarianceRos = poseCovarianceRos.block<3, 3>(3, 3).diagonal(); + + std::thread publishNonTimeCriticalDataThread(&GraphMsfRos2::publishNonTimeCriticalData, this, poseCovarianceRos, + twistCovarianceRos, positionVarianceRos, orientationVarianceRos, + integratedNavStatePtr, optimizedStateWithCovarianceAndBiasPtr); + publishNonTimeCriticalDataThread.detach(); +} + +void GraphMsfRos2::publishNonTimeCriticalData( + const Eigen::Matrix poseCovarianceRos, const Eigen::Matrix twistCovarianceRos, + const Eigen::Vector3d positionVarianceRos, const Eigen::Vector3d orientationVarianceRos, + const std::shared_ptr integratedNavStatePtr, + const std::shared_ptr optimizedStateWithCovarianceAndBiasPtr) { + std::lock_guard lock(rosPublisherMutex_); + + double timeK = integratedNavStatePtr->getTimeK(); + + publishImuOdoms(integratedNavStatePtr, poseCovarianceRos, twistCovarianceRos); + publishDiagVarianceVectors(positionVarianceRos, orientationVarianceRos, timeK); + publishVelocityMarkers(integratedNavStatePtr); + publishImuPaths(integratedNavStatePtr); + publishOptimizedStateAndBias(optimizedStateWithCovarianceAndBiasPtr, poseCovarianceRos, twistCovarianceRos); +} + +} // namespace graph_msf diff --git a/ros2/graph_msf_ros2/src/lib/StaticTransformsTf.cpp b/ros2/graph_msf_ros2/src/lib/StaticTransformsTf.cpp new file mode 100644 index 00000000..be823232 --- /dev/null +++ b/ros2/graph_msf_ros2/src/lib/StaticTransformsTf.cpp @@ -0,0 +1,64 @@ +/* +Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +// Implementation +#include "graph_msf_ros2/extrinsics/StaticTransformsTf.h" + +// ROS +// #include +#include +#include +#include + +// Workspace +#include "graph_msf_ros2/constants.h" +#include "graph_msf_ros2/util/conversions.h" + +namespace graph_msf { + +StaticTransformsTf::StaticTransformsTf(const rclcpp::Node::SharedPtr& node, + const graph_msf::StaticTransforms& staticTransforms) + : StaticTransforms(staticTransforms), + tf_buffer_(std::make_shared(node->get_clock())), + tf_listener_(*tf_buffer_) { + RCLCPP_INFO(rclcpp::get_logger("graph_msf"), "StaticTransformsTf - Initializing static transforms..."); +} + +void StaticTransformsTf::findTransformations() { + RCLCPP_INFO(rclcpp::get_logger("graph_msf"), "Looking up transforms in TF-tree."); + RCLCPP_INFO(rclcpp::get_logger("graph_msf"), "Transforms between the following frames are required: %s, %s", + imuFrame_.c_str(), baseLinkFrame_.c_str()); + RCLCPP_INFO(rclcpp::get_logger("graph_msf"), "Waiting for up to 100 seconds until they arrive..."); + + // Temporary variable + geometry_msgs::msg::TransformStamped transform_stamped; + + // Imu to Base --- + RCLCPP_INFO(rclcpp::get_logger("graph_msf"), "Looking up transform from %s to %s", imuFrame_.c_str(), + baseLinkFrame_.c_str()); + try { + transform_stamped = + tf_buffer_->lookupTransform(imuFrame_, baseLinkFrame_, rclcpp::Time(0), rclcpp::Duration::from_seconds(100.0)); + tf2::Transform tf_transform; + graph_msf::transformStampedToIsometry3(transform_stamped.transform, lv_T_frame1_frame2(imuFrame_, baseLinkFrame_)); + + // graph_msf::tfToIsometry3(tf_transform, lv_T_frame1_frame2(imuFrame_, baseLinkFrame_)); + // graph_msf::tfToIsometry3(transform, lv_T_frame1_frame2(imuFrame_, baseLinkFrame_)); + // RCLCPP_INFO(rclcpp::get_logger("graph_msf"), "Translation I_Base: %s", + // lv_T_frame1_frame2(imuFrame_, baseLinkFrame_) + // .translation() + // .format(Eigen::IOFormat(Eigen::StreamPrecision)) + // .c_str()); + lv_T_frame1_frame2(baseLinkFrame_, imuFrame_) = lv_T_frame1_frame2(imuFrame_, baseLinkFrame_).inverse(); + } catch (const tf2::TransformException& ex) { + RCLCPP_ERROR(rclcpp::get_logger("graph_msf"), "Transform lookup failed: %s", ex.what()); + } + + RCLCPP_INFO(rclcpp::get_logger("graph_msf"), "Transforms looked up successfully."); +} + +} // namespace graph_msf diff --git a/ros2/graph_msf_ros2/src/lib/StaticTransformsUrdf.cpp b/ros2/graph_msf_ros2/src/lib/StaticTransformsUrdf.cpp new file mode 100644 index 00000000..80b7243a --- /dev/null +++ b/ros2/graph_msf_ros2/src/lib/StaticTransformsUrdf.cpp @@ -0,0 +1,84 @@ +/* +Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +#include "graph_msf_ros2/extrinsics/StaticTransformsUrdf.h" +#include + +namespace graph_msf { + +StaticTransformsUrdf::StaticTransformsUrdf(const std::shared_ptr privateNodePtr) + : privateNode_(privateNodePtr) { + RCLCPP_INFO(privateNode_->get_logger(), "StaticTransformsUrdf Initializing..."); +} + +void StaticTransformsUrdf::setup() { + RCLCPP_INFO(privateNode_->get_logger(), "Setting up for description name '%s'", urdfDescriptionName_.c_str()); + + // Declare and retrieve the URDF parameter + privateNode_->declare_parameter(urdfDescriptionName_, "default_description"); + std::string urdfDescriptionContent; + privateNode_->get_parameter(urdfDescriptionName_, urdfDescriptionContent); + + if (urdfDescriptionContent.empty()) { + throw std::runtime_error("StaticTransformsUrdf - Could not load description, description empty."); + } + + // Initialize the urdf::Model from the URDF string content + if (!urdfModel_.initString(urdfDescriptionContent)) { + throw std::runtime_error("StaticTransformsUrdf - Failed to parse URDF description content."); + } + + // Initialize the KDL tree + if (!kdl_parser::treeFromUrdfModel(urdfModel_, tree_)) { + throw std::runtime_error("Failed to extract KDL tree from robot description"); + } + + segments_.clear(); + getRootTransformations(tree_.getRootSegment()); + + RCLCPP_INFO(privateNode_->get_logger(), "StaticTransformsUrdf Initialized."); +} + +void StaticTransformsUrdf::getRootTransformations(const KDL::SegmentMap::const_iterator element, std::string rootName) { + const std::string& elementName = GetTreeElementSegment(element->second).getName(); + if (rootName.empty()) { + rootName = elementName; + } + + auto children = GetTreeElementChildren(element->second); + for (const auto& child : children) { + KDL::Chain chain; + tree_.getChain(rootName, child->second.segment.getName(), chain); + + tf2::Transform T_root_element; + for (int i = 0; i < chain.getNrOfSegments(); ++i) { + KDL::Frame frameToRoot = chain.getSegment(i).getFrameToTip(); + double x, y, z, w; + frameToRoot.M.GetQuaternion(x, y, z, w); + tf2::Quaternion q(x, y, z, w); + + tf2::Vector3 t(frameToRoot.p[0], frameToRoot.p[1], frameToRoot.p[2]); + T_root_element = T_root_element * tf2::Transform(q, t); + } + + ElementToRoot elementToRoot(T_root_element, rootName, child->second.segment.getName()); + segments_.emplace(child->second.segment.getName(), elementToRoot); + getRootTransformations(child, rootName); + } +} + +tf2::Transform StaticTransformsUrdf::kdlToTransform(const KDL::Frame& k) { + tf2::Transform tf_T; + tf_T.setOrigin(tf2::Vector3(k.p.x(), k.p.y(), k.p.z())); + tf2::Quaternion q; + k.M.GetQuaternion(q[0], q[1], q[2], q[3]); + tf_T.setRotation(q); + + return tf_T; +} + +} // namespace graph_msf diff --git a/ros2/graph_msf_ros2/src/lib/conversions.cpp b/ros2/graph_msf_ros2/src/lib/conversions.cpp new file mode 100644 index 00000000..9ecc6139 --- /dev/null +++ b/ros2/graph_msf_ros2/src/lib/conversions.cpp @@ -0,0 +1,98 @@ +// Implementation +#include "graph_msf_ros2/util/conversions.h" +#include + +namespace graph_msf { + +Eigen::Matrix convertCovarianceGtsamConventionToRosConvention( + const Eigen::Matrix& covGtsam) { + Eigen::Matrix covRos; + covRos.setZero(); + covRos.block<3, 3>(0, 0) = covGtsam.block<3, 3>(3, 3); + covRos.block<3, 3>(3, 3) = covGtsam.block<3, 3>(0, 0); + covRos.block<3, 3>(0, 3) = covGtsam.block<3, 3>(3, 0); + covRos.block<3, 3>(3, 0) = covGtsam.block<3, 3>(0, 3); + return covRos; +} + +void geometryPoseToEigen(const geometry_msgs::msg::Pose& pose, Eigen::Matrix4d& T) { + Eigen::Affine3d affine = Eigen::Affine3d::Identity(); + affine.translation() = Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z); + Eigen::Quaterniond q(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); + affine.linear() = q.toRotationMatrix(); + T = affine.matrix(); +} + +void geometryPoseToEigen(const geometry_msgs::msg::PoseStamped& pose, Eigen::Matrix4d& T) { + geometryPoseToEigen(pose.pose, T); +} + +void geometryPoseToEigen(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, Eigen::Matrix4d& T) { + geometryPoseToEigen(pose.pose.pose, T); +} + +void odomMsgToEigen(const nav_msgs::msg::Odometry& odomLidar, Eigen::Matrix4d& T) { + Eigen::Affine3d affine = Eigen::Affine3d::Identity(); + + affine.translation() = + Eigen::Vector3d(odomLidar.pose.pose.position.x, odomLidar.pose.pose.position.y, odomLidar.pose.pose.position.z); + Eigen::Quaterniond q(odomLidar.pose.pose.orientation.w, odomLidar.pose.pose.orientation.x, + odomLidar.pose.pose.orientation.y, odomLidar.pose.pose.orientation.z); + affine.linear() = q.toRotationMatrix(); + T = affine.matrix(); +} + +void odomMsgToTf(const nav_msgs::msg::Odometry& odomLidar, tf2::Transform& T) { + T = tf2::Transform( + tf2::Quaternion(odomLidar.pose.pose.orientation.x, odomLidar.pose.pose.orientation.y, + odomLidar.pose.pose.orientation.z, odomLidar.pose.pose.orientation.w), + tf2::Vector3(odomLidar.pose.pose.position.x, odomLidar.pose.pose.position.y, odomLidar.pose.pose.position.z)); +} + +tf2::Transform matrix3ToTf(const Eigen::Matrix3d& R) { + Eigen::Quaterniond q(R); + return tf2::Transform(tf2::Quaternion(q.x(), q.y(), q.z(), q.w()), tf2::Vector3(0.0, 0.0, 0.0)); +} + +tf2::Transform matrix4ToTf(const Eigen::Matrix4d& T) { + Eigen::Quaterniond q(T.block<3, 3>(0, 0)); + return tf2::Transform(tf2::Quaternion(q.x(), q.y(), q.z(), q.w()), tf2::Vector3(T(0, 3), T(1, 3), T(2, 3))); +} + +tf2::Transform isometry3ToTf(const Eigen::Isometry3d& T) { + Eigen::Quaterniond q(T.rotation()); + return tf2::Transform(tf2::Quaternion(q.x(), q.y(), q.z(), q.w()), + tf2::Vector3(T.translation().x(), T.translation().y(), T.translation().z())); +} + +#include +#include + +void tfToMatrix4(const tf2::Transform& tf_T, Eigen::Matrix4d& T) { + Eigen::Affine3d affine = Eigen::Affine3d::Identity(); + affine.translation() = Eigen::Vector3d(tf_T.getOrigin().x(), tf_T.getOrigin().y(), tf_T.getOrigin().z()); + Eigen::Quaterniond q(tf_T.getRotation().w(), tf_T.getRotation().x(), tf_T.getRotation().y(), tf_T.getRotation().z()); + affine.linear() = q.toRotationMatrix(); + T = affine.matrix(); +} + +void tfToIsometry3(const tf2::Transform& tf_T, Eigen::Isometry3d& T) { + tf2::Vector3 origin = tf_T.getOrigin(); + T.translation() << origin.x(), origin.y(), origin.z(); + tf2::Quaternion tf_q = tf_T.getRotation(); + Eigen::Quaterniond eigen_q(tf_q.w(), tf_q.x(), tf_q.y(), tf_q.z()); + T.linear() = eigen_q.toRotationMatrix(); +} + +void transformStampedToIsometry3(const geometry_msgs::msg::Transform& transform, Eigen::Isometry3d& T) { + T.translation() = Eigen::Vector3d(transform.translation.x, transform.translation.y, transform.translation.z); + Eigen::Quaterniond q(transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z); + T.linear() = q.toRotationMatrix(); +} + +tf2::Transform pose3ToTf(const Eigen::Matrix3d& T) { + Eigen::Quaterniond q(T); + return tf2::Transform(tf2::Quaternion(q.x(), q.y(), q.z(), q.w()), tf2::Vector3(0.0, 0.0, 0.0)); +} + +} // namespace graph_msf diff --git a/ros2/graph_msf_ros2/src/lib/readParams.cpp b/ros2/graph_msf_ros2/src/lib/readParams.cpp new file mode 100644 index 00000000..a0232773 --- /dev/null +++ b/ros2/graph_msf_ros2/src/lib/readParams.cpp @@ -0,0 +1,157 @@ +/* +Copyright 2024 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +node_ file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of node_ package. + */ + +// C++ +#include + +// Implementation +#include "graph_msf_ros2/GraphMsfRos2.h" + +// Workspace +#include "graph_msf_ros2/ros/read_ros_params.h" + +namespace graph_msf { + +void GraphMsfRos2::readParams() { + RCLCPP_INFO(node_->get_logger(), "GraphMsfRos2 - Reading parameters."); + + if (!graphConfigPtr_) { + throw std::runtime_error("GraphMsfRos2: graphConfigPtr must be initialized."); + } + + // Configuration + // Sensor Parameters + graphConfigPtr_->imuRate_ = tryGetParam(node_, "sensor_params.imuRate"); + graphConfigPtr_->createStateEveryNthImuMeasurement_ = + tryGetParam(node_, "sensor_params.createStateEveryNthImuMeasurement"); + graphConfigPtr_->useImuSignalLowPassFilter_ = tryGetParam(node_, "sensor_params.useImuSignalLowPassFilter"); + graphConfigPtr_->imuLowPassFilterCutoffFreqHz_ = + tryGetParam(node_, "sensor_params.imuLowPassFilterCutoffFreq"); + graphConfigPtr_->maxSearchDeviation_ = + 1.0 / (graphConfigPtr_->imuRate_ / graphConfigPtr_->createStateEveryNthImuMeasurement_); + graphConfigPtr_->imuBufferLength_ = tryGetParam(node_, "sensor_params.imuBufferLength"); + graphConfigPtr_->imuTimeOffset_ = tryGetParam(node_, "sensor_params.imuTimeOffset"); + + // Initialization Params + graphConfigPtr_->estimateGravityFromImuFlag_ = + tryGetParam(node_, "initialization_params.estimateGravityFromImu"); + graphConfigPtr_->gravityMagnitude_ = tryGetParam(node_, "initialization_params.gravityMagnitude"); + graphConfigPtr_->W_gravityVector_ = Eigen::Vector3d(0.0, 0.0, -1.0) * graphConfigPtr_->gravityMagnitude_; + + // Graph Params + graphConfigPtr_->realTimeSmootherUseIsamFlag_ = tryGetParam(node_, "graph_params.realTimeSmootherUseIsam"); + graphConfigPtr_->realTimeSmootherLag_ = tryGetParam(node_, "graph_params.realTimeSmootherLag"); + graphConfigPtr_->useAdditionalSlowBatchSmootherFlag_ = + tryGetParam(node_, "graph_params.useAdditionalSlowBatchSmoother"); + graphConfigPtr_->slowBatchSmootherUseIsamFlag_ = tryGetParam(node_, "graph_params.slowBatchSmootherUseIsam"); + + // Optimizer Config + graphConfigPtr_->gaussNewtonWildfireThreshold_ = + tryGetParam(node_, "graph_params.gaussNewtonWildfireThreshold"); + graphConfigPtr_->minOptimizationFrequency_ = tryGetParam(node_, "graph_params.minOptimizationFrequency"); + graphConfigPtr_->maxOptimizationFrequency_ = tryGetParam(node_, "graph_params.maxOptimizationFrequency"); + graphConfigPtr_->additionalOptimizationIterations_ = + tryGetParam(node_, "graph_params.additionalOptimizationIterations"); + graphConfigPtr_->findUnusedFactorSlotsFlag_ = tryGetParam(node_, "graph_params.findUnusedFactorSlots"); + graphConfigPtr_->enableDetailedResultsFlag_ = tryGetParam(node_, "graph_params.enableDetailedResults"); + graphConfigPtr_->realTimeSmootherUseCholeskyFactorizationFlag_ = + tryGetParam(node_, "graph_params.realTimeSmootherUseCholeskyFactorization"); + graphConfigPtr_->slowBatchSmootherUseCholeskyFactorizationFlag_ = + tryGetParam(node_, "graph_params.slowBatchSmootherUseCholeskyFactorization"); + graphConfigPtr_->usingBiasForPreIntegrationFlag_ = + tryGetParam(node_, "graph_params.usingBiasForPreIntegration"); + graphConfigPtr_->optimizeReferenceFramePosesWrtWorldFlag_ = + tryGetParam(node_, "graph_params.optimizeReferenceFramePosesWrtWorld"); + graphConfigPtr_->optimizeExtrinsicSensorToSensorCorrectedOffsetFlag_ = + tryGetParam(node_, "graph_params.optimizeExtrinsicSensorToSensorCorrectedOffset"); + + // Alignment Parameters + graphConfigPtr_->referenceFramePosesResetThreshold_ = + tryGetParam(node_, "graph_params.referenceFramePosesResetThreshold"); +// graphConfigPtr_->centerReferenceFramesAtRobotPositionBeforeAlignment_ = +// tryGetParam(node_, "graph_params.centerReferenceFramesAtRobotPositionBeforeAlignment"); + + // Noise Parameters + graphConfigPtr_->accNoiseDensity_ = tryGetParam(node_, "noise_params.accNoiseDensity"); + graphConfigPtr_->integrationNoiseDensity_ = tryGetParam(node_, "noise_params.integrationNoiseDensity"); + graphConfigPtr_->use2ndOrderCoriolisFlag_ = tryGetParam(node_, "noise_params.use2ndOrderCoriolis"); + graphConfigPtr_->gyroNoiseDensity_ = tryGetParam(node_, "noise_params.gyrNoiseDensity"); + graphConfigPtr_->omegaCoriolis_ = tryGetParam(node_, "noise_params.omegaCoriolis"); + graphConfigPtr_->accBiasRandomWalkNoiseDensity_ = + tryGetParam(node_, "noise_params.accBiasRandomWalkNoiseDensity"); + graphConfigPtr_->gyroBiasRandomWalkNoiseDensity_ = + tryGetParam(node_, "noise_params.gyrBiasRandomWalkNoiseDensity"); + graphConfigPtr_->biasAccOmegaInit_ = tryGetParam(node_, "noise_params.biasAccOmegaInit"); + + const double accBiasPrior = tryGetParam(node_, "noise_params.accBiasPrior"); + graphConfigPtr_->accBiasPrior_ = Eigen::Vector3d(accBiasPrior, accBiasPrior, accBiasPrior); + + const double gyroBiasPrior = tryGetParam(node_, "noise_params.gyrBiasPrior"); + graphConfigPtr_->gyroBiasPrior_ = Eigen::Vector3d(gyroBiasPrior, gyroBiasPrior, gyroBiasPrior); + + // Initial State + graphConfigPtr_->initialPositionNoiseDensity_ = + tryGetParam(node_, "noise_params.initialPositionNoiseDensity"); + graphConfigPtr_->initialOrientationNoiseDensity_ = + tryGetParam(node_, "noise_params.initialOrientationNoiseDensity"); + graphConfigPtr_->initialVelocityNoiseDensity_ = + tryGetParam(node_, "noise_params.initialVelocityNoiseDensity"); + graphConfigPtr_->initialAccBiasNoiseDensity_ = tryGetParam(node_, "noise_params.initialAccBiasNoiseDensity"); + graphConfigPtr_->initialGyroBiasNoiseDensity_ = + tryGetParam(node_, "noise_params.initialGyroBiasNoiseDensity"); + + // Re-linearization + graphConfigPtr_->positionReLinTh_ = tryGetParam(node_, "relinearization_params.positionReLinTh"); + graphConfigPtr_->rotationReLinTh_ = tryGetParam(node_, "relinearization_params.rotationReLinTh"); + graphConfigPtr_->velocityReLinTh_ = tryGetParam(node_, "relinearization_params.velocityReLinTh"); + graphConfigPtr_->accBiasReLinTh_ = tryGetParam(node_, "relinearization_params.accBiasReLinTh"); + graphConfigPtr_->gyroBiasReLinTh_ = tryGetParam(node_, "relinearization_params.gyrBiasReLinTh"); + graphConfigPtr_->referenceFrameReLinTh_ = tryGetParam(node_, "relinearization_params.referenceFrameReLinTh"); + graphConfigPtr_->calibrationReLinTh_ = tryGetParam(node_, "relinearization_params.calibrationReLinTh"); + graphConfigPtr_->displacementReLinTh_ = tryGetParam(node_, "relinearization_params.displacementReLinTh"); + graphConfigPtr_->landmarkReLinTh_ = tryGetParam(node_, "relinearization_params.landmarkReLinTh"); + + graphConfigPtr_->relinearizeSkip_ = tryGetParam(node_, "relinearization_params.relinearizeSkip"); + graphConfigPtr_->enableRelinearizationFlag_ = + tryGetParam(node_, "relinearization_params.enableRelinearization"); + graphConfigPtr_->evaluateNonlinearErrorFlag_ = + tryGetParam(node_, "relinearization_params.evaluateNonlinearError"); + graphConfigPtr_->cacheLinearizedFactorsFlag_ = + tryGetParam(node_, "relinearization_params.cacheLinearizedFactors"); + graphConfigPtr_->enablePartialRelinearizationCheckFlag_ = + tryGetParam(node_, "relinearization_params.enablePartialRelinearizationCheck"); + + // Common Parameters + graphConfigPtr_->verboseLevel_ = tryGetParam(node_, "common_params.verbosity"); + graphConfigPtr_->odomNotJumpAtStartFlag_ = tryGetParam(node_, "common_params.odomNotJumpAtStart"); + + // Set frames + staticTransformsPtr_->setWorldFrame(tryGetParam(node_, "extrinsics.worldFrame")); + staticTransformsPtr_->setOdomFrame(tryGetParam(node_, "extrinsics.odomFrame")); + staticTransformsPtr_->setImuFrame(tryGetParam(node_, "extrinsics.imuFrame")); + staticTransformsPtr_->setInitializationFrame( + tryGetParam(node_, "extrinsics.initializeZeroYawAndPositionOfFrame")); + staticTransformsPtr_->setBaseLinkFrame(tryGetParam(node_, "extrinsics.baseLinkFrame")); + + referenceFrameAlignedNameId = tryGetParam(node_, "name_ids.referenceFrameAligned"); + sensorFrameCorrectedNameId = tryGetParam(node_, "name_ids.sensorFrameCorrected"); + + // Logging path + if (graphConfigPtr_->useAdditionalSlowBatchSmootherFlag_) { + optimizationResultLoggingPath = tryGetParam(node_, "launch.optimizationResultLoggingPath"); + + if (optimizationResultLoggingPath.back() != '/') { + optimizationResultLoggingPath += "/"; + } + + if (!std::filesystem::exists(optimizationResultLoggingPath)) { + std::filesystem::create_directories(optimizationResultLoggingPath); + } + } +} + +} // namespace graph_msf diff --git a/ros2/graph_msf_ros2/srv/OfflineOptimizationTrigger.srv b/ros2/graph_msf_ros2/srv/OfflineOptimizationTrigger.srv new file mode 100644 index 00000000..34507c11 --- /dev/null +++ b/ros2/graph_msf_ros2/srv/OfflineOptimizationTrigger.srv @@ -0,0 +1,4 @@ +int64 max_optimization_iterations +--- +bool success +string message \ No newline at end of file diff --git a/ros2/graph_msf_ros2_msgs/CMakeLists.txt b/ros2/graph_msf_ros2_msgs/CMakeLists.txt new file mode 100644 index 00000000..2f6e3df2 --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.16) +project(graph_msf_ros2_msgs) + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(nav_msgs REQUIRED) + +# Message and Service Files +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Wgs84Coordinate.msg" + "srv/GetPathInEnu.srv" + DEPENDENCIES nav_msgs +) + +# Package export and installation +ament_export_dependencies(rosidl_default_runtime nav_msgs) +ament_package() diff --git a/ros2/graph_msf_ros2_msgs/msg/Wgs84Coordinate.msg b/ros2/graph_msf_ros2_msgs/msg/Wgs84Coordinate.msg new file mode 100644 index 00000000..20c74042 --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/msg/Wgs84Coordinate.msg @@ -0,0 +1,3 @@ +float64 longitude +float64 latitude +float64 altitude \ No newline at end of file diff --git a/ros2/graph_msf_ros2_msgs/package.xml b/ros2/graph_msf_ros2_msgs/package.xml new file mode 100644 index 00000000..0b4b3276 --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/package.xml @@ -0,0 +1,26 @@ + + + graph_msf_ros2_msgs + 1.0.0 + Messages used by graph_msf + + Proprietary + https://bitbucket.org/leggedrobotics/compslam_se + Gabriel Waibel + Gabriel Waibel + + ament_cmake + rosidl_default_generators + + rosidl_default_runtime + nav_msgs + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/ros2/graph_msf_ros2_msgs/srv/GetPathInEnu.srv b/ros2/graph_msf_ros2_msgs/srv/GetPathInEnu.srv new file mode 100644 index 00000000..93177abf --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/srv/GetPathInEnu.srv @@ -0,0 +1,3 @@ +Wgs84Coordinate[] wgs84_coordinates +--- +nav_msgs/Path gnss_enu_path \ No newline at end of file From 8d3740411ca22c95346a0b7d7b8772931201176c Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Mon, 19 May 2025 10:26:52 +0000 Subject: [PATCH 02/60] temporaliy edit cmakelist and package xml --- graph_msf/CMakeLists.txt | 128 +++++++++++++++++---------------------- graph_msf/package.xml | 18 +++--- 2 files changed, 65 insertions(+), 81 deletions(-) diff --git a/graph_msf/CMakeLists.txt b/graph_msf/CMakeLists.txt index f7ebac21..c1ec3f59 100644 --- a/graph_msf/CMakeLists.txt +++ b/graph_msf/CMakeLists.txt @@ -1,101 +1,83 @@ cmake_minimum_required(VERSION 3.16) project(graph_msf) -## Compile as C++17, supported in ROS Noetic and newer add_compile_options(-std=c++17) # Find dependencies ---------------------------------------------------------------------------------------------------- find_package(Eigen3 REQUIRED) -message("Eigen Version::" ${EIGEN3_VERSION_STRING}) -message("Eigen Path::" ${Eigen3_DIR}) +find_package(ament_cmake REQUIRED) +find_package(GTSAM REQUIRED) +find_package(GTSAM_UNSTABLE REQUIRED) +find_package(Python3 REQUIRED COMPONENTS Development) +find_package(tf2_eigen REQUIRED) -# Color -if (NOT WIN32) +message("Eigen Version:: ${EIGEN3_VERSION_STRING}") +message("Eigen Path:: ${EIGEN3_DIR}") +message("GTSAM Include Path:: ${GTSAM_INCLUDE_DIRS}") + +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +# Color settings for terminal output +if(NOT WIN32) string(ASCII 27 Esc) set(ColourReset "${Esc}[m") set(BoldMagenta "${Esc}[1;35m") - set(Magenta "${Esc}[35m") -endif () - -# March native check -#include(CheckCXXCompilerFlag) -#CHECK_CXX_COMPILER_FLAG("-march=native" COMPILER_SUPPORTS_MARCH_NATIVE) -#if(COMPILER_SUPPORTS_MARCH_NATIVE) -# add_compile_options("-march=native") -# message("${BoldMagenta}INFO: Using -march=native${ColourReset}") -#endif() - -# Catkin -set(CATKIN_PACKAGE_DEPENDENCIES - gtsam_catkin -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -catkin_package( - CATKIN_DEPENDS ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} -) - -########### -## Build ## -########### -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(Magenta "${Esc}[35m") +endif() include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} - # ${GTSAM_INCLUDE_DIR} - ${Python3_INCLUDE_DIRS} + include + SYSTEM ${GTSAM_INCLUDE_DIR} + SYSTEM ${GTSAM_UNSTABLE_INCLUDE_DIR} + ${EIGEN3_INCLUDE_DIR} + ${Python3_INCLUDE_DIRS} ) -add_library(${PROJECT_NAME} - src/lib/eigen_wrapped_gtsam_utils.cpp - src/lib/FileLogger.cpp - src/lib/GraphMsf.cpp - src/lib/GraphMsfClassic.cpp - src/lib/GraphMsfHolistic.cpp - src/lib/GraphManager.cpp - src/lib/ImuBuffer.cpp - src/lib/Gnss.cpp - src/lib/GnssHandler.cpp - src/lib/TimeGraphKeyBuffer.cpp - src/lib/TrajectoryAlignment.cpp - src/lib/TrajectoryAlignmentHandler.cpp - src/lib/NavState.cpp +add_library(${PROJECT_NAME} SHARED + src/lib/eigen_wrapped_gtsam_utils.cpp + src/lib/FileLogger.cpp + src/lib/GraphMsf.cpp + src/lib/GraphMsfClassic.cpp + src/lib/GraphMsfHolistic.cpp + src/lib/GraphManager.cpp + src/lib/ImuBuffer.cpp + src/lib/Gnss.cpp + src/lib/GnssHandler.cpp + src/lib/TimeGraphKeyBuffer.cpp + src/lib/TrajectoryAlignment.cpp + src/lib/TrajectoryAlignmentHandler.cpp + src/lib/NavState.cpp ) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) - -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +# Link dependencies explicitly +ament_target_dependencies(${PROJECT_NAME} Eigen3 GTSAM GTSAM_UNSTABLE tf2_eigen Python3) +target_link_libraries( graph_msf gtsam gtsam_unstable) -# Add clang tooling +# Optional: Clang tooling find_package(cmake_clang_tools QUIET) if (cmake_clang_tools_FOUND AND NOT DEFINED NO_CLANG_TOOLING) add_clang_tooling( - TARGET ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_FIX + TARGET ${PROJECT_NAME} + SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include + CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include + CF_FIX ) -endif (cmake_clang_tools_FOUND AND NOT DEFINED NO_CLANG_TOOLING) - -############# -## Install ## -############# +endif() +# Install targets and directories -------------------------------------------------------------------------------------- install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION include/${PROJECT_NAME} ) + +# Export package settings for ament ------------------------------------------------------------------------------------ +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(Eigen3 GTSAM GTSAM_UNSTABLE tf2_eigen Python3) + +ament_package() diff --git a/graph_msf/package.xml b/graph_msf/package.xml index 6738ffe6..2ba74281 100644 --- a/graph_msf/package.xml +++ b/graph_msf/package.xml @@ -12,14 +12,16 @@ BSD Julian Nubert - catkin + ament_cmake + rosidl_default_generators + + tf2_eigen + std_srvs cmake_clang_tools - Eigen3 - GTSAM - gtsam_catkin + eigen_colcon + gtsam - Eigen3 - GTSAM - gtsam_catkin + eigen_colcon + gtsam - + \ No newline at end of file From 66d4f1ff00617c3fdb3b76d54a7fc0bcf974d764 Mon Sep 17 00:00:00 2001 From: Julian Date: Tue, 20 May 2025 17:48:23 +0200 Subject: [PATCH 03/60] noetic docker setup --- .gitignore | 2 + catkin_workspace.vcs | 21 ++++ docker/bin/attach_to_latest_container.sh | 18 +++ docker/bin/run_ros1_noetic_image.sh | 64 ++++++++++ docker/ros1_noetic/README.md | 13 ++ docker/ros1_noetic/base.Dockerfile | 79 ++++++++++++ docker/ros1_noetic/bashrc | 149 +++++++++++++++++++++++ docker/ros1_noetic/entrypoint.sh | 31 +++++ docker/submodules/base.sh | 39 ++++++ docker/submodules/gtsam.sh | 17 +++ docker/submodules/ros_1.sh | 44 +++++++ 11 files changed, 477 insertions(+) create mode 100644 catkin_workspace.vcs create mode 100755 docker/bin/attach_to_latest_container.sh create mode 100755 docker/bin/run_ros1_noetic_image.sh create mode 100644 docker/ros1_noetic/README.md create mode 100644 docker/ros1_noetic/base.Dockerfile create mode 100644 docker/ros1_noetic/bashrc create mode 100644 docker/ros1_noetic/entrypoint.sh create mode 100644 docker/submodules/base.sh create mode 100644 docker/submodules/gtsam.sh create mode 100644 docker/submodules/ros_1.sh diff --git a/.gitignore b/.gitignore index be7487a4..9fecd79f 100644 --- a/.gitignore +++ b/.gitignore @@ -68,3 +68,5 @@ Thumbs.db *.mov *.wmv +# Local directories +.etc/ diff --git a/catkin_workspace.vcs b/catkin_workspace.vcs new file mode 100644 index 00000000..57b5b739 --- /dev/null +++ b/catkin_workspace.vcs @@ -0,0 +1,21 @@ +repositories: + elevation_mapping: + type: git + url: https://github.com/leggedrobotics/elevation_mapping.git + version: master + gtsam_catkin: + type: git + url: https://github.com/leggedrobotics/gtsam_catkin.git + version: main + holistic_fusion: + type: git + url: git@github.com:leggedrobotics/holistic_fusion.git + version: feature/docker + kindr: + type: git + url: https://github.com/leggedrobotics/kindr.git + version: master + kindr_ros: + type: git + url: https://github.com/leggedrobotics/kindr_ros.git + version: master diff --git a/docker/bin/attach_to_latest_container.sh b/docker/bin/attach_to_latest_container.sh new file mode 100755 index 00000000..6a469699 --- /dev/null +++ b/docker/bin/attach_to_latest_container.sh @@ -0,0 +1,18 @@ +#!/bin/bash + +#============================================================================= +# Copyright (C) 2025, Robotic Systems Lab, ETH Zurich +# All rights reserved. +# http://www.rsl.ethz.ch +# +# This software is distributed WITHOUT ANY WARRANTY; without even the +# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the License for more information. +#============================================================================= +# Authors: Julian Nubert, nubertj@ethz.ch +# Lorenzo Terenzi, lterenzi@ethz.ch +#============================================================================= + +LATEST_CONTAINER_NAME=$(docker ps --latest --format "{{.Names}}") +echo "Name of latest docker container is $LATEST_CONTAINER_NAME. Attaching to it..." +docker exec -it $LATEST_CONTAINER_NAME /entrypoint.sh \ No newline at end of file diff --git a/docker/bin/run_ros1_noetic_image.sh b/docker/bin/run_ros1_noetic_image.sh new file mode 100755 index 00000000..19d927cd --- /dev/null +++ b/docker/bin/run_ros1_noetic_image.sh @@ -0,0 +1,64 @@ +#!/bin/bash + +#============================================================================= +# Copyright (C) 2025, Robotic Systems Lab, ETH Zurich +# All rights reserved. +# http://www.rsl.ethz.ch +# +# This software is distributed WITHOUT ANY WARRANTY; without even the +# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the License for more information. +#============================================================================= +# Authors: Julian Nubert, nubertj@ethz.ch +# Lorenzo Terenzi, lterenzi@ethz.ch +#============================================================================= + +# Exits if error occurs +set -e + +# Image name +IMAGE="rslethz/holistic_fusion_ros1:latest" + +# Entry point +ENTRYPOINT="/entrypoint.sh" + +# Graphical output +XSOCK=/tmp/.X11-unix +XAUTH=/tmp/.docker.xauth + +# Create symlinks to user configs within the build context. +mkdir -p .etc && cd .etc +ln -sf /etc/passwd . +ln -sf /etc/shadow . +ln -sf /etc/group . +cd .. + +# Launch a container from the prebuilt image. +echo -e "[run.sh]: \e[1;32mRunning docker image '$IMAGE' with entrypoint $ENTRYPOINT.\e[0m" +echo "---------------------" +RUN_COMMAND="docker run \ + $ARGS \ + -v /lib/modules:/lib/modules \ + --volume=$XSOCK:$XSOCK:rw \ + --volume=$XAUTH:$XAUTH:rw \ + --env="QT_X11_NO_MITSHM=1" \ + --env="XAUTHORITY=$XAUTH" \ + --env="DISPLAY=$DISPLAY" \ + --cap-add=ALL \ + --privileged \ + --net=host \ + -eHOST_USERNAME=$(whoami) \ + -v$HOME:$HOME \ + -v$(pwd)/.etc/shadow:/etc/shadow \ + -v$(pwd)/.etc/passwd:/etc/passwd \ + -v$(pwd)/.etc/group:/etc/group \ + -v/etc/localtime:/etc/localtime:ro \ + --entrypoint=$ENTRYPOINT + --shm-size=2gb + -it $IMAGE" + +# Final command +echo -e "[run.sh]: \e[1;32mThe final run command is\n\e[0;35m$RUN_COMMAND\e[0m." +$RUN_COMMAND + +# EOF \ No newline at end of file diff --git a/docker/ros1_noetic/README.md b/docker/ros1_noetic/README.md new file mode 100644 index 00000000..ac8e5ce1 --- /dev/null +++ b/docker/ros1_noetic/README.md @@ -0,0 +1,13 @@ +# Docker Setup for Holistic Fusion + +We provide docker images for running Holistic Fusion in a controlled environment. +The ROS1 Image is based on Ubuntu 20.04 and includes the necessary dependencies for running Holistic Fusion. +The ROS2 Image is based on Ubuntu 22.04 and will be released in the future. + +## ROS1 Noetic Image + +To build the ROS1 Noetic image, run the following command from the root of the repository: + +```bash +docker/bin/build_ros1_noetic_image.sh +``` \ No newline at end of file diff --git a/docker/ros1_noetic/base.Dockerfile b/docker/ros1_noetic/base.Dockerfile new file mode 100644 index 00000000..81de6d32 --- /dev/null +++ b/docker/ros1_noetic/base.Dockerfile @@ -0,0 +1,79 @@ +#============================================================================= +# Copyright (C) 2025, Robotic Systems Lab, ETH Zurich +# All rights reserved. +# http://www.rsl.ethz.ch +# +# This software is distributed WITHOUT ANY WARRANTY; without even the +# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the License for more information. +#============================================================================= +# Authors: Julian Nubert, nubertj@ethz.ch +# Lorenzo Terenzi, lterenzi@ethz.ch +#============================================================================= + +#== +# Foundation +#== +ARG UBUNTU_VERSION=20.04 +FROM ubuntu:${UBUNTU_VERSION} + +# Suppresses interactive calls to APT +ENV DEBIAN_FRONTEND="noninteractive" + +# Updates +RUN apt update && apt upgrade -y + +# ---------------------------------------------------------------------------- + +#== +# System APT base dependencies and utilities +#== + +COPY submodules/base.sh /home/base.sh +RUN chmod +x /home/base.sh +RUN /home/base.sh && rm /home/base.sh + +#== +# ROS +#== + +# Version +ARG ROS=noetic + +COPY submodules/ros_1.sh /home/ros_1.sh +RUN chmod +x /home/ros_1.sh +RUN /home/ros_1.sh && rm /home/ros_1.sh + +#== +# GTSAM +#== + +COPY submodules/gtsam.sh /home/gtsam.sh +RUN chmod +x /home/gtsam.sh +RUN /home/gtsam.sh && rm /home/gtsam.sh + +# ---------------------------------------------------------------------------- + +#== +# Permissions +#== +RUN chmod ugo+rwx /software + +#== +# Environment +#== +COPY ros1_noetic/bashrc /etc/bash.bashrc +RUN chmod a+rwx /etc/bash.bashrc + +# ---------------------------------------------------------------------------- + +#== +# Execution +#== + +COPY ros1_noetic/entrypoint.sh /entrypoint.sh +RUN chmod +x /entrypoint.sh +#ENTRYPOINT ["/entrypoint.sh"] +CMD [] + +# EOF \ No newline at end of file diff --git a/docker/ros1_noetic/bashrc b/docker/ros1_noetic/bashrc new file mode 100644 index 00000000..46d3a5b6 --- /dev/null +++ b/docker/ros1_noetic/bashrc @@ -0,0 +1,149 @@ +#!/bin/bash + +#============================================================================= +# Copyright (C) 2025, Robotic Systems Lab, ETH Zurich +# All rights reserved. +# http://www.rsl.ethz.ch +# +# This software is distributed WITHOUT ANY WARRANTY; without even the +# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the License for more information. +#============================================================================= +# Authors: Julian Nubert, nubertj@ethz.ch +# Lorenzo Terenzi, lterenzi@ethz.ch +#============================================================================= + +# If not running interactively, don't do anything +case $- in + *i*) ;; + *) return;; +esac + +# don't put duplicate lines or lines starting with space in the history. +HISTCONTROL=ignoreboth + +# append to the history file, don't overwrite it +shopt -s histappend + +# for setting history length see HISTSIZE and HISTFILESIZE in bash(1) +HISTSIZE=1000 +HISTFILESIZE=2000 + +# check the window size after each command and, if necessary, +# update the values of LINES and COLUMNS. +shopt -s checkwinsize + +# make less more friendly for non-text input files, see lesspipe(1) +[ -x /usr/bin/lesspipe ] && eval "$(SHELL=/bin/sh lesspipe)" + +# set variable identifying the chroot you work in (used in the prompt below) +if [ -z "${debian_chroot:-}" ] && [ -r /etc/debian_chroot ]; then + debian_chroot=$(cat /etc/debian_chroot) +fi + +# set a fancy prompt (non-color, unless we know we "want" color) +case "$TERM" in + xterm-color|*-256color) color_prompt=yes;; +esac + +# uncomment for a colored prompt, if the terminal has the capability; turned +# off by default to not distract the user: the focus in a terminal window +# should be on the output of commands, not on the prompt +#force_color_prompt=yes + +if [ -n "$force_color_prompt" ]; then + if [ -x /usr/bin/tput ] && tput setaf 1 >&/dev/null; then + # We have color support; assume it's compliant with Ecma-48 + # (ISO/IEC-6429). (Lack of such support is extremely rare, and such + # a case would tend to support setf rather than setaf.) + color_prompt=yes + else + color_prompt= + fi +fi + +if [ "$color_prompt" = yes ]; then + PS1='${debian_chroot:+($debian_chroot)}\[\033[01;32m\]\u@\h\[\033[00m\]:\[\033[01;34m\]\w\[\033[00m\]\$ ' +else + PS1='${debian_chroot:+($debian_chroot)}\u@\h:\w\$ ' +fi +unset color_prompt force_color_prompt + +# If this is an xterm set the title to user@host:dir +case "$TERM" in +xterm*|rxvt*) + PS1="\[\e]0;${debian_chroot:+($debian_chroot)}\u@\h: \w\a\]$PS1" + ;; +*) + ;; +esac + +# enable color support of ls and also add handy aliases +if [ -x /usr/bin/dircolors ]; then + test -r ~/.dircolors && eval "$(dircolors -b ~/.dircolors)" || eval "$(dircolors -b)" + alias ls='ls --color=auto' + alias grep='grep --color=auto' + alias fgrep='fgrep --color=auto' + alias egrep='egrep --color=auto' +fi + +# some more ls aliases +alias ll='ls -alF' +alias la='ls -A' +alias l='ls -CF' + +# Add an "alert" alias for long running commands. Use like so: +# sleep 10; alert +alias alert='notify-send --urgency=low -i "$([ $? = 0 ] && echo terminal || echo error)" "$(history|tail -n1|sed -e '\''s/^\s*[0-9]\+\s*//;s/[;&|]\s*alert$//'\'')"' + +# Alias definitions. +if [ -f ~/.bash_aliases ]; then + . ~/.bash_aliases +fi + +# enable programmable completion features (you don't need to enable +# this, if it's already enabled in /etc/bash.bashrc and /etc/profile +# sources /etc/bash.bashrc). +if ! shopt -oq posix; then + if [ -f /usr/share/bash-completion/bash_completion ]; then + . /usr/share/bash-completion/bash_completion + elif [ -f /etc/bash_completion ]; then + . /etc/bash_completion + fi +fi + +#== +# Fuzzy search +#== +# Install fuzzy-search if missing +if [[ ! -d ~/.fzf ]] +then + git clone --depth 1 https://github.com/junegunn/fzf.git ~/.fzf && ~/.fzf/install +fi + +# Enable fuzzy search in terminal +[ -f ~/.fzf.bash ] && source ~/.fzf.bash + +#== +# Git +#== +parse_git_branch() { + git branch 2> /dev/null | sed -e '/^[^*]/d' -e 's/* \(.*\)/(\1)/' +} +export PS1=$PS1"\[\e[91m\]\$(parse_git_branch)\[\e[00m\]$ " +export PS1="(D) "$PS1 + +#== +# ROS +#== +# Source the ROS distribution file to configure the shell environment. +FILE=/opt/ros/noetic/setup.bash +if test -f "$FILE"; +then + echo "$FILE exists. Therefore sourcing noetic." + source $FILE +else + echo "$FILE does not exist. Therefore not sourcing noetic." +fi + +# EOF \ No newline at end of file diff --git a/docker/ros1_noetic/entrypoint.sh b/docker/ros1_noetic/entrypoint.sh new file mode 100644 index 00000000..82c14dbe --- /dev/null +++ b/docker/ros1_noetic/entrypoint.sh @@ -0,0 +1,31 @@ +#!/bin/bash + +#============================================================================= +# Copyright (C) 2025, Robotic Systems Lab, ETH Zurich +# All rights reserved. +# http://www.rsl.ethz.ch +# +# This software is distributed WITHOUT ANY WARRANTY; without even the +# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the License for more information. +#============================================================================= +# Authors: Vassilios Tsounis, tsounisv@ethz.ch +# Julian Nubert, nubertj@ethz.ch +#============================================================================= +figlet m545 +#== +# Log into the container as the host user +#== +# set home for host user +export HOME=/home/$HOST_USERNAME +export USER=$HOST_USERNAME +cd $HOME + +# Enable sudo access without password +echo "root ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers +echo "$HOST_USERNAME ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers + +# Proceed as host user with superuser permissions +sudo -E -u $HOST_USERNAME bash --rcfile /etc/bash.bashrc + +# EOF \ No newline at end of file diff --git a/docker/submodules/base.sh b/docker/submodules/base.sh new file mode 100644 index 00000000..2aca9603 --- /dev/null +++ b/docker/submodules/base.sh @@ -0,0 +1,39 @@ +#!/bin/bash + +apt update && apt install -y \ + sudo \ + tzdata \ + lsb-release \ + ca-certificates \ + apt-utils \ + gnupg2 \ + locate \ + curl \ + wget \ + git \ + vim \ + gedit \ + tmux \ + unzip \ + iputils-ping \ + net-tools \ + htop \ + iotop \ + iftop \ + nmap \ + software-properties-common \ + build-essential \ + gdb \ + pkg-config \ + cmake \ + zsh \ + clang-format \ + clang-tidy \ + xterm \ + gnome-terminal \ + dialog \ + tasksel \ + meld \ + figlet \ + && \ + rm -rf /var/lib/apt/lists/* \ No newline at end of file diff --git a/docker/submodules/gtsam.sh b/docker/submodules/gtsam.sh new file mode 100644 index 00000000..3c7fc92e --- /dev/null +++ b/docker/submodules/gtsam.sh @@ -0,0 +1,17 @@ +#!/bin/bash + +mkdir -p /software \ + && cd /software \ + && git clone https://github.com/borglab/gtsam.git \ + && mkdir -p /software/gtsam/build \ + && cd /software/gtsam/build \ + && git checkout 4.2 \ + && cmake \ + -DCMAKE_BUILD_TYPE=Release \ + -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \ + -DGTSAM_POSE3_EXPMAP=ON \ + -DGTSAM_ROT3_EXPMAP=ON \ + -DGTSAM_USE_QUATERNIONS=ON \ + -DGTSAM_USE_SYSTEM_EIGEN=ON \ + /software/gtsam \ + && make install -j$(nproc) \ No newline at end of file diff --git a/docker/submodules/ros_1.sh b/docker/submodules/ros_1.sh new file mode 100644 index 00000000..4d03ddce --- /dev/null +++ b/docker/submodules/ros_1.sh @@ -0,0 +1,44 @@ +#!/bin/bash + +sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' \ + && curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - \ + && apt update && apt install -y \ + python3-pip \ + python3-rosdep \ + python3-rosclean \ + python3-rosparam \ + python3-progressbar \ + python3-catkin-tools \ + python3-osrf-pycommon \ + python3-virtualenvwrapper \ + ros-${ROS}-desktop-full \ + ros-${ROS}-velodyne-pointcloud \ + ros-${ROS}-joy \ + ros-${ROS}-grid-map-core \ + ros-${ROS}-grid-map \ + && rm -rf /var/lib/apt/lists/* \ + && rosdep init && rosdep update \ + && apt update && apt install -y \ + libblas-dev \ + xutils-dev \ + gfortran \ + libf2c2-dev \ + libgmock-dev \ + libgoogle-glog-dev \ + libboost-all-dev \ + libeigen3-dev \ + libglpk-dev \ + liburdfdom-dev \ + liboctomap-dev \ + libassimp-dev \ + python3-catkin-tools \ + python3-pytest \ + python3-lxml \ + ros-${ROS}-ompl \ + ros-${ROS}-octomap-msgs \ + ros-${ROS}-pybind11-catkin \ + doxygen-latex \ + usbutils \ + python3-vcstool \ + && rm -rf /var/lib/apt/lists/* \ + && sudo ln -s /usr/include/eigen3 /usr/local/include/ \ No newline at end of file From c9ae4bcff4d29cedb367d2d7eec4d28ce98d78b8 Mon Sep 17 00:00:00 2001 From: Julian Date: Tue, 20 May 2025 18:57:14 +0200 Subject: [PATCH 04/60] first non running version of graph_msf_catkin --- graph_msf/CMakeLists.txt | 46 ++++++-------------- graph_msf/package.xml | 3 -- ros/graph_msf_catkin/.gitignore | 3 ++ ros/graph_msf_catkin/CMakeLists.txt | 66 +++++++++++++++++++++++++++++ ros/graph_msf_catkin/package.xml | 12 ++++++ 5 files changed, 94 insertions(+), 36 deletions(-) create mode 100644 ros/graph_msf_catkin/.gitignore create mode 100644 ros/graph_msf_catkin/CMakeLists.txt create mode 100644 ros/graph_msf_catkin/package.xml diff --git a/graph_msf/CMakeLists.txt b/graph_msf/CMakeLists.txt index f7ebac21..c3640e04 100644 --- a/graph_msf/CMakeLists.txt +++ b/graph_msf/CMakeLists.txt @@ -17,29 +17,10 @@ if (NOT WIN32) set(Magenta "${Esc}[35m") endif () -# March native check -#include(CheckCXXCompilerFlag) -#CHECK_CXX_COMPILER_FLAG("-march=native" COMPILER_SUPPORTS_MARCH_NATIVE) -#if(COMPILER_SUPPORTS_MARCH_NATIVE) -# add_compile_options("-march=native") -# message("${BoldMagenta}INFO: Using -march=native${ColourReset}") -#endif() - -# Catkin -set(CATKIN_PACKAGE_DEPENDENCIES - gtsam_catkin -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -catkin_package( - CATKIN_DEPENDS ${CATKIN_PACKAGE_DEPENDENCIES} - DEPENDS - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} -) +# Eigen +find_package(Eigen3 REQUIRED COMPONENTS) +# GTSAM +find_package(GTSAM REQUIRED) ########### ## Build ## @@ -49,10 +30,8 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) include_directories( include - ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} - # ${GTSAM_INCLUDE_DIR} - ${Python3_INCLUDE_DIRS} + ${GTSAM_INCLUDE_DIR} ) add_library(${PROJECT_NAME} @@ -71,9 +50,10 @@ add_library(${PROJECT_NAME} src/lib/NavState.cpp ) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) - -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${GTSAM_LIBRARIES} +) # Add clang tooling find_package(cmake_clang_tools QUIET) @@ -91,11 +71,11 @@ endif (cmake_clang_tools_FOUND AND NOT DEFINED NO_CLANG_TOOLING) ############# install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} ) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/${PROJECT_NAME} ) diff --git a/graph_msf/package.xml b/graph_msf/package.xml index 6738ffe6..7b0e9b41 100644 --- a/graph_msf/package.xml +++ b/graph_msf/package.xml @@ -12,14 +12,11 @@ BSD Julian Nubert - catkin cmake_clang_tools Eigen3 GTSAM - gtsam_catkin Eigen3 GTSAM - gtsam_catkin diff --git a/ros/graph_msf_catkin/.gitignore b/ros/graph_msf_catkin/.gitignore new file mode 100644 index 00000000..c9b104d3 --- /dev/null +++ b/ros/graph_msf_catkin/.gitignore @@ -0,0 +1,3 @@ +# Temp +tmp/ +src/ \ No newline at end of file diff --git a/ros/graph_msf_catkin/CMakeLists.txt b/ros/graph_msf_catkin/CMakeLists.txt new file mode 100644 index 00000000..ad38e95f --- /dev/null +++ b/ros/graph_msf_catkin/CMakeLists.txt @@ -0,0 +1,66 @@ +cmake_minimum_required(VERSION 3.10) +project(graph_msf_catkin) + +find_package(catkin REQUIRED) + +# Color +if (NOT WIN32) + string(ASCII 27 Esc) + set(ColourReset "${Esc}[m") + set(BoldMagenta "${Esc}[1;35m") + set(Magenta "${Esc}[35m") +endif () + +# Print CMAKE_BINARY_DIR +message("CMAKE_BINARY_DIR: ${CMAKE_BINARY_DIR}") +message("CMAKE_SOURCE_DIR: ${CMAKE_SOURCE_DIR}") +# Print CMAKE_DEVEL_PREFIX +message("CATKIN_DEVEL_PREFIX: ${CATKIN_DEVEL_PREFIX}") +# Print CATKIN_GLOBAL_INCLUDE_DESTINATION +message("CATKIN_GLOBAL_INCLUDE_DESTINATION: ${CATKIN_GLOBAL_INCLUDE_DESTINATION}") +# Print CATKIN_GLOBAL_LIB_DESTINATION +message("CATKIN_GLOBAL_LIB_DESTINATION: ${CATKIN_GLOBAL_LIB_DESTINATION}") + +# Graph MSF +include(ExternalProject) +file(MAKE_DIRECTORY ${CATKIN_DEVEL_PREFIX}/include) + +ExternalProject_Add(graph_msf + SOURCE_DIR ${CMAKE_SOURCE_DIR}/../../graph_msf + PREFIX "${CMAKE_SOURCE_DIR}" + TMP_DIR "${CMAKE_BINARY_DIR}/graph_msf/tmp" + BINARY_DIR "${CMAKE_BINARY_DIR}/graph_msf/build" + INSTALL_DIR "${CATKIN_DEVEL_PREFIX}" + CMAKE_CACHE_ARGS "-DCMAKE_POSITION_INDEPENDENT_CODE:BOOL=true" + CMAKE_ARGS + -DCMAKE_BUILD_TYPE=Release + -DCMAKE_INSTALL_PREFIX=${CATKIN_DEVEL_PREFIX} + BUILD_ALWAYS 1 +) + +# Dependencies --------------------------------------------------------- +add_dependencies(graph_msf ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) + +############# +## INSTALL ## +############# + +install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} + ) + +install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib/ + DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION} + ) + +# Final catkin package -------------------------------------------------- +catkin_package( + INCLUDE_DIRS + ${CATKIN_DEVEL_PREFIX}/include + LIBRARIES + ${CATKIN_DEVEL_PREFIX}/lib/libgraph_msf.so + ${CS_PROJECT_LIBRARIES} + CATKIN_DEPENDS + ${${PROJECT_NAME}_CATKIN_RUN_DEPENDS} + ${CATKIN_PACKAGE_DEPENDENCIES} +) diff --git a/ros/graph_msf_catkin/package.xml b/ros/graph_msf_catkin/package.xml new file mode 100644 index 00000000..f7455d0e --- /dev/null +++ b/ros/graph_msf_catkin/package.xml @@ -0,0 +1,12 @@ + + graph_msf_catkin + 0.0.1 + + Catkin wrapper for native CMake project graph_msf + + Julian Nubert + BSD + Julian Nubert + + catkin + From 734373c57846adf4b0ae65763aa40b1f524e0677 Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Wed, 21 May 2025 15:25:11 +0000 Subject: [PATCH 05/60] Add initial implementation of smb_estimator_graph_ros2 package --- .../smb_estimator_graph_ros2/CMakeLists.txt | 124 +++ .../smb_estimator_graph_ros2/FindGflags.cmake | 591 ++++++++++++++ .../smb_estimator_graph_ros2/FindGlog.cmake | 346 +++++++++ .../compslam/compslam_lio_RS16_params.yaml | 109 +++ .../config/core/core_extrinsic_params.yaml | 14 + .../config/core/core_graph_config.yaml | 6 + .../config/core/core_graph_params.yaml | 79 ++ .../dumped_params/2023-06-13-13-15-57.yaml | 523 +++++++++++++ .../smb_specific/smb_extrinsic_params.yaml | 7 + .../config/smb_specific/smb_graph_params.yaml | 30 + .../smb_estimator_graph_ros2/SmbEstimator.h | 124 +++ .../SmbStaticTransforms.h | 49 ++ .../smb_estimator_graph_ros2/constants.h | 15 + .../launch/smb_estimator_graph.launch | 48 ++ .../launch/smb_estimator_graph_replay.launch | 22 + .../ros2/smb_estimator_graph_ros2/package.xml | 32 + .../rviz/lidar_estimation.rviz | 729 ++++++++++++++++++ .../scripts/remove_map_and_odom_from_bag.py | 56 ++ .../src/lib/SmbEstimator.cpp | 312 ++++++++ .../src/lib/SmbStaticTransforms.cpp | 102 +++ .../src/lib/readParams.cpp | 92 +++ .../src/smb_estimator_node.cpp | 36 + 22 files changed, 3446 insertions(+) create mode 100644 examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt create mode 100644 examples/ros2/smb_estimator_graph_ros2/FindGflags.cmake create mode 100644 examples/ros2/smb_estimator_graph_ros2/FindGlog.cmake create mode 100644 examples/ros2/smb_estimator_graph_ros2/config/compslam/compslam_lio_RS16_params.yaml create mode 100644 examples/ros2/smb_estimator_graph_ros2/config/core/core_extrinsic_params.yaml create mode 100644 examples/ros2/smb_estimator_graph_ros2/config/core/core_graph_config.yaml create mode 100644 examples/ros2/smb_estimator_graph_ros2/config/core/core_graph_params.yaml create mode 100644 examples/ros2/smb_estimator_graph_ros2/config/dumped_params/2023-06-13-13-15-57.yaml create mode 100644 examples/ros2/smb_estimator_graph_ros2/config/smb_specific/smb_extrinsic_params.yaml create mode 100644 examples/ros2/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml create mode 100644 examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbEstimator.h create mode 100644 examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h create mode 100644 examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/constants.h create mode 100644 examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch create mode 100644 examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch create mode 100644 examples/ros2/smb_estimator_graph_ros2/package.xml create mode 100644 examples/ros2/smb_estimator_graph_ros2/rviz/lidar_estimation.rviz create mode 100644 examples/ros2/smb_estimator_graph_ros2/scripts/remove_map_and_odom_from_bag.py create mode 100644 examples/ros2/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp create mode 100644 examples/ros2/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp create mode 100644 examples/ros2/smb_estimator_graph_ros2/src/lib/readParams.cpp create mode 100644 examples/ros2/smb_estimator_graph_ros2/src/smb_estimator_node.cpp diff --git a/examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt b/examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt new file mode 100644 index 00000000..7fd88e52 --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt @@ -0,0 +1,124 @@ +cmake_minimum_required(VERSION 3.16) +project(smb_estimator_graph_ros2) + +## Compile as C++17 +add_compile_options(-std=c++17) +set(CMAKE_CXX_FLAGS_RELEASE "-O3") + +# For FindGlog.cmake and FindGflags.cmake +list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}") + +# Find Dependencies --------------------------------------------------------------------------------------------------- +find_package(ament_cmake REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(graph_msf_ros2 REQUIRED) +find_package(graph_msf REQUIRED) +find_package(Glog REQUIRED) + +# Display Eigen version and path +message("Eigen Version: ${EIGEN3_VERSION_STRING}") +message("Eigen Path: ${Eigen3_DIR}") + +# Color +if(NOT WIN32) + string(ASCII 27 Esc) + set(ColourReset "${Esc}[m") + set(BoldMagenta "${Esc}[1;35m") + set(Magenta "${Esc}[35m") +endif() + +# Define include directories +include_directories( + include + ${EIGEN3_INCLUDE_DIR} + ${GLOG_INCLUDE_DIRS} + ${graph_msf_ros2_INCLUDE_DIRS} + ${graph_msf_INCLUDE_DIRS} +) + +# Library +add_library(${PROJECT_NAME} + src/lib/SmbEstimator.cpp + src/lib/readParams.cpp + src/lib/SmbStaticTransforms.cpp + include/smb_estimator_graph/constants.h +) + +target_link_libraries(${PROJECT_NAME} + ${gflags_LIBRARIES} + ${GLOG_LIBRARIES} + ${graph_msf_LIBRARIES} + ${graph_msf_ros2_LIBRARIES} +) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + std_msgs + sensor_msgs + nav_msgs + tf2 + tf2_ros + graph_msf + graph_msf_ros2 +) + +# Executable +add_executable(${PROJECT_NAME}_node src/smb_estimator_node.cpp) +target_link_libraries(${PROJECT_NAME}_node + ${PROJECT_NAME} + ${gflags_LIBRARIES} + ${GLOG_LIBRARIES} + ${graph_msf_LIBRARIES} + ${graph_msf_ros2_LIBRARIES} +) + +ament_target_dependencies(${PROJECT_NAME}_node + rclcpp + std_msgs + sensor_msgs + nav_msgs + tf2 + tf2_ros + graph_msf + graph_msf_ros2 +) + +# Add clang tooling +find_package(cmake_clang_tools QUIET) +if(cmake_clang_tools_FOUND AND NOT DEFINED NO_CLANG_TOOLING) + add_clang_tooling( + TARGET ${PROJECT_NAME} + SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include + CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include + CF_FIX + ) +endif() + +# Install targets +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(TARGETS ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} +) + +# Mark other files for installation +install(DIRECTORY launch rviz config + DESTINATION share/${PROJECT_NAME} +) + +# Finalize ament package +ament_package() diff --git a/examples/ros2/smb_estimator_graph_ros2/FindGflags.cmake b/examples/ros2/smb_estimator_graph_ros2/FindGflags.cmake new file mode 100644 index 00000000..7ad5b202 --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/FindGflags.cmake @@ -0,0 +1,591 @@ +# Ceres Solver - A fast non-linear least squares minimizer +# Copyright 2015 Google Inc. All rights reserved. +# http://ceres-solver.org/ +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Google Inc. nor the names of its contributors may be +# used to endorse or promote products derived from this software without +# specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: alexs.mac@gmail.com (Alex Stewart) +# + +# FindGflags.cmake - Find Google gflags logging library. +# +# This module will attempt to find gflags, either via an exported CMake +# configuration (generated by gflags >= 2.1 which are built with CMake), or +# by performing a standard search for all gflags components. The order of +# precedence for these two methods of finding gflags is controlled by: +# GFLAGS_PREFER_EXPORTED_GFLAGS_CMAKE_CONFIGURATION. +# +# This module defines the following variables: +# +# GFLAGS_FOUND: TRUE iff gflags is found. +# GFLAGS_INCLUDE_DIRS: Include directories for gflags. +# GFLAGS_LIBRARIES: Libraries required to link gflags. +# GFLAGS_NAMESPACE: The namespace in which gflags is defined. In versions of +# gflags < 2.1, this was google, for versions >= 2.1 it is +# by default gflags, although can be configured when building +# gflags to be something else (i.e. google for legacy +# compatibility). +# FOUND_INSTALLED_GFLAGS_CMAKE_CONFIGURATION: True iff the version of gflags +# found was built & installed / +# exported as a CMake package. +# +# The following variables control the behaviour of this module when an exported +# gflags CMake configuration is not found. +# +# GFLAGS_PREFER_EXPORTED_GFLAGS_CMAKE_CONFIGURATION: TRUE/FALSE, iff TRUE then +# then prefer using an exported CMake configuration +# generated by gflags >= 2.1 over searching for the +# gflags components manually. Otherwise (FALSE) +# ignore any exported gflags CMake configurations and +# always perform a manual search for the components. +# Default: TRUE iff user does not define this variable +# before we are called, and does NOT specify either +# GFLAGS_INCLUDE_DIR_HINTS or GFLAGS_LIBRARY_DIR_HINTS +# otherwise FALSE. +# GFLAGS_INCLUDE_DIR_HINTS: List of additional directories in which to +# search for gflags includes, e.g: /timbuktu/include. +# GFLAGS_LIBRARY_DIR_HINTS: List of additional directories in which to +# search for gflags libraries, e.g: /timbuktu/lib. +# +# The following variables are also defined by this module, but in line with +# CMake recommended FindPackage() module style should NOT be referenced directly +# by callers (use the plural variables detailed above instead). These variables +# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which +# are NOT re-called (i.e. search for library is not repeated) if these variables +# are set with valid values _in the CMake cache_. This means that if these +# variables are set directly in the cache, either by the user in the CMake GUI, +# or by the user passing -DVAR=VALUE directives to CMake when called (which +# explicitly defines a cache variable), then they will be used verbatim, +# bypassing the HINTS variables and other hard-coded search locations. +# +# GFLAGS_INCLUDE_DIR: Include directory for gflags, not including the +# include directory of any dependencies. +# GFLAGS_LIBRARY: gflags library, not including the libraries of any +# dependencies. + +# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when FindGflags was +# invoked, necessary for MSVC. +macro(GFLAGS_RESET_FIND_LIBRARY_PREFIX) + if (MSVC AND CALLERS_CMAKE_FIND_LIBRARY_PREFIXES) + set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}") + endif() +endmacro(GFLAGS_RESET_FIND_LIBRARY_PREFIX) + +# Called if we failed to find gflags or any of it's required dependencies, +# unsets all public (designed to be used externally) variables and reports +# error message at priority depending upon [REQUIRED/QUIET/] argument. +macro(GFLAGS_REPORT_NOT_FOUND REASON_MSG) + unset(GFLAGS_FOUND) + unset(GFLAGS_INCLUDE_DIRS) + unset(GFLAGS_LIBRARIES) + # Do not use unset, as we want to keep GFLAGS_NAMESPACE in the cache, + # but simply clear its value. + set(GFLAGS_NAMESPACE "" CACHE STRING + "gflags namespace (google or gflags)" FORCE) + + # Make results of search visible in the CMake GUI if gflags has not + # been found so that user does not have to toggle to advanced view. + mark_as_advanced(CLEAR GFLAGS_INCLUDE_DIR + GFLAGS_LIBRARY + GFLAGS_NAMESPACE) + + gflags_reset_find_library_prefix() + + # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() + # use the camelcase library name, not uppercase. + if (Gflags_FIND_QUIETLY) + message(STATUS "Failed to find gflags - " ${REASON_MSG} ${ARGN}) + elseif (Gflags_FIND_REQUIRED) + message(FATAL_ERROR "Failed to find gflags - " ${REASON_MSG} ${ARGN}) + else() + # Neither QUIETLY nor REQUIRED, use no priority which emits a message + # but continues configuration and allows generation. + message("-- Failed to find gflags - " ${REASON_MSG} ${ARGN}) + endif () + return() +endmacro(GFLAGS_REPORT_NOT_FOUND) + +# Verify that all variable names passed as arguments are defined (can be empty +# but must be defined) or raise a fatal error. +macro(GFLAGS_CHECK_VARS_DEFINED) + foreach(CHECK_VAR ${ARGN}) + if (NOT DEFINED ${CHECK_VAR}) + message(FATAL_ERROR "open3d_slam Bug: ${CHECK_VAR} is not defined.") + endif() + endforeach() +endmacro(GFLAGS_CHECK_VARS_DEFINED) + +# Use check_cxx_source_compiles() to compile trivial test programs to determine +# the gflags namespace. This works on all OSs except Windows. If using Visual +# Studio, it fails because msbuild forces check_cxx_source_compiles() to use +# CMAKE_BUILD_TYPE=Debug for the test project, which usually breaks detection +# because MSVC requires that the test project use the same build type as gflags, +# which would normally be built in Release. +# +# Defines: GFLAGS_NAMESPACE in the caller's scope with the detected namespace, +# which is blank (empty string, will test FALSE is CMake conditionals) +# if detection failed. +function(GFLAGS_CHECK_GFLAGS_NAMESPACE_USING_TRY_COMPILE) + # Verify that all required variables are defined. + gflags_check_vars_defined( + GFLAGS_INCLUDE_DIR GFLAGS_LIBRARY) + # Ensure that GFLAGS_NAMESPACE is always unset on completion unless + # we explicitly set if after having the correct namespace. + set(GFLAGS_NAMESPACE "" PARENT_SCOPE) + + include(CheckCXXSourceCompiles) + # Setup include path & link library for gflags for CHECK_CXX_SOURCE_COMPILES. + set(CMAKE_REQUIRED_INCLUDES ${GFLAGS_INCLUDE_DIR}) + set(CMAKE_REQUIRED_LIBRARIES ${GFLAGS_LIBRARY} ${GFLAGS_LINK_LIBRARIES}) + # First try the (older) google namespace. Note that the output variable + # MUST be unique to the build type as otherwise the test is not repeated as + # it is assumed to have already been performed. + check_cxx_source_compiles( + "#include + int main(int argc, char * argv[]) { + google::ParseCommandLineFlags(&argc, &argv, true); + return 0; + }" + GFLAGS_IN_GOOGLE_NAMESPACE) + if (GFLAGS_IN_GOOGLE_NAMESPACE) + set(GFLAGS_NAMESPACE google PARENT_SCOPE) + return() + endif() + + # Try (newer) gflags namespace instead. Note that the output variable + # MUST be unique to the build type as otherwise the test is not repeated as + # it is assumed to have already been performed. + set(CMAKE_REQUIRED_INCLUDES ${GFLAGS_INCLUDE_DIR}) + set(CMAKE_REQUIRED_LIBRARIES ${GFLAGS_LIBRARY} ${GFLAGS_LINK_LIBRARIES}) + check_cxx_source_compiles( + "#include + int main(int argc, char * argv[]) { + gflags::ParseCommandLineFlags(&argc, &argv, true); + return 0; + }" + GFLAGS_IN_GFLAGS_NAMESPACE) + if (GFLAGS_IN_GFLAGS_NAMESPACE) + set(GFLAGS_NAMESPACE gflags PARENT_SCOPE) + return() + endif (GFLAGS_IN_GFLAGS_NAMESPACE) +endfunction(GFLAGS_CHECK_GFLAGS_NAMESPACE_USING_TRY_COMPILE) + +# Use regex on the gflags headers to attempt to determine the gflags namespace. +# Checks both gflags.h (contained namespace on versions < 2.1.2) and +# gflags_declare.h, which contains the namespace on versions >= 2.1.2. +# In general, this method should only be used when +# GFLAGS_CHECK_GFLAGS_NAMESPACE_USING_TRY_COMPILE() cannot be used, or has +# failed. +# +# Defines: GFLAGS_NAMESPACE in the caller's scope with the detected namespace, +# which is blank (empty string, will test FALSE is CMake conditionals) +# if detection failed. +function(GFLAGS_CHECK_GFLAGS_NAMESPACE_USING_REGEX) + # Verify that all required variables are defined. + gflags_check_vars_defined(GFLAGS_INCLUDE_DIR) + # Ensure that GFLAGS_NAMESPACE is always undefined on completion unless + # we explicitly set if after having the correct namespace. + set(GFLAGS_NAMESPACE "" PARENT_SCOPE) + + # Scan gflags.h to identify what namespace gflags was built with. On + # versions of gflags < 2.1.2, gflags.h was configured with the namespace + # directly, on >= 2.1.2, gflags.h uses the GFLAGS_NAMESPACE #define which + # is defined in gflags_declare.h, we try each location in turn. + set(GFLAGS_HEADER_FILE ${GFLAGS_INCLUDE_DIR}/gflags/gflags.h) + if (NOT EXISTS ${GFLAGS_HEADER_FILE}) + gflags_report_not_found( + "Could not find file: ${GFLAGS_HEADER_FILE} " + "containing namespace information in gflags install located at: " + "${GFLAGS_INCLUDE_DIR}.") + endif() + file(READ ${GFLAGS_HEADER_FILE} GFLAGS_HEADER_FILE_CONTENTS) + + string(REGEX MATCH "namespace [A-Za-z]+" + GFLAGS_NAMESPACE "${GFLAGS_HEADER_FILE_CONTENTS}") + string(REGEX REPLACE "namespace ([A-Za-z]+)" "\\1" + GFLAGS_NAMESPACE "${GFLAGS_NAMESPACE}") + + if (NOT GFLAGS_NAMESPACE) + gflags_report_not_found( + "Failed to extract gflags namespace from header file: " + "${GFLAGS_HEADER_FILE}.") + endif (NOT GFLAGS_NAMESPACE) + + if (GFLAGS_NAMESPACE STREQUAL "google" OR + GFLAGS_NAMESPACE STREQUAL "gflags") + # Found valid gflags namespace from gflags.h. + set(GFLAGS_NAMESPACE "${GFLAGS_NAMESPACE}" PARENT_SCOPE) + return() + endif() + + # Failed to find gflags namespace from gflags.h, gflags is likely a new + # version, check gflags_declare.h, which in newer versions (>= 2.1.2) contains + # the GFLAGS_NAMESPACE #define, which is then referenced in gflags.h. + set(GFLAGS_DECLARE_FILE ${GFLAGS_INCLUDE_DIR}/gflags/gflags_declare.h) + if (NOT EXISTS ${GFLAGS_DECLARE_FILE}) + gflags_report_not_found( + "Could not find file: ${GFLAGS_DECLARE_FILE} " + "containing namespace information in gflags install located at: " + "${GFLAGS_INCLUDE_DIR}.") + endif() + file(READ ${GFLAGS_DECLARE_FILE} GFLAGS_DECLARE_FILE_CONTENTS) + + string(REGEX MATCH "#define GFLAGS_NAMESPACE [A-Za-z]+" + GFLAGS_NAMESPACE "${GFLAGS_DECLARE_FILE_CONTENTS}") + string(REGEX REPLACE "#define GFLAGS_NAMESPACE ([A-Za-z]+)" "\\1" + GFLAGS_NAMESPACE "${GFLAGS_NAMESPACE}") + + if (NOT GFLAGS_NAMESPACE) + gflags_report_not_found( + "Failed to extract gflags namespace from declare file: " + "${GFLAGS_DECLARE_FILE}.") + endif (NOT GFLAGS_NAMESPACE) + + if (GFLAGS_NAMESPACE STREQUAL "google" OR + GFLAGS_NAMESPACE STREQUAL "gflags") + # Found valid gflags namespace from gflags.h. + set(GFLAGS_NAMESPACE "${GFLAGS_NAMESPACE}" PARENT_SCOPE) + return() + endif() +endfunction(GFLAGS_CHECK_GFLAGS_NAMESPACE_USING_REGEX) + +# Protect against any alternative find_package scripts for this library having +# been called previously (in a client project) which set GFLAGS_FOUND, but not +# the other variables we require / set here which could cause the search logic +# here to fail. +unset(GFLAGS_FOUND) + +# ----------------------------------------------------------------- +# By default, if the user has expressed no preference for using an exported +# gflags CMake configuration over performing a search for the installed +# components, and has not specified any hints for the search locations, then +# prefer a gflags exported configuration if available. +if (NOT DEFINED GFLAGS_PREFER_EXPORTED_GFLAGS_CMAKE_CONFIGURATION + AND NOT GFLAGS_INCLUDE_DIR_HINTS + AND NOT GFLAGS_LIBRARY_DIR_HINTS) + message(STATUS "No preference for use of exported gflags CMake configuration " + "set, and no hints for include/library directories provided. " + "Defaulting to preferring an installed/exported gflags CMake configuration " + "if available.") + set(GFLAGS_PREFER_EXPORTED_GFLAGS_CMAKE_CONFIGURATION TRUE) +endif() + +if (GFLAGS_PREFER_EXPORTED_GFLAGS_CMAKE_CONFIGURATION) + # Try to find an exported CMake configuration for gflags, as generated by + # gflags versions >= 2.1. + # + # We search twice, s/t we can invert the ordering of precedence used by + # find_package() for exported package build directories, and installed + # packages (found via CMAKE_SYSTEM_PREFIX_PATH), listed as items 6) and 7) + # respectively in [1]. + # + # By default, exported build directories are (in theory) detected first, and + # this is usually the case on Windows. However, on OS X & Linux, the install + # path (/usr/local) is typically present in the PATH environment variable + # which is checked in item 4) in [1] (i.e. before both of the above, unless + # NO_SYSTEM_ENVIRONMENT_PATH is passed). As such on those OSs installed + # packages are usually detected in preference to exported package build + # directories. + # + # To ensure a more consistent response across all OSs, and as users usually + # want to prefer an installed version of a package over a locally built one + # where both exist (esp. as the exported build directory might be removed + # after installation), we first search with NO_CMAKE_PACKAGE_REGISTRY which + # means any build directories exported by the user are ignored, and thus + # installed directories are preferred. If this fails to find the package + # we then research again, but without NO_CMAKE_PACKAGE_REGISTRY, so any + # exported build directories will now be detected. + # + # To prevent confusion on Windows, we also pass NO_CMAKE_BUILDS_PATH (which + # is item 5) in [1]), to not preferentially use projects that were built + # recently with the CMake GUI to ensure that we always prefer an installed + # version if available. + # + # [1] http://www.cmake.org/cmake/help/v2.8.11/cmake.html#command:find_package + find_package(gflags QUIET + NO_MODULE + NO_CMAKE_PACKAGE_REGISTRY + NO_CMAKE_BUILDS_PATH) + if (gflags_FOUND) + message(STATUS "Found installed version of gflags: ${gflags_DIR}") + else(gflags_FOUND) + # Failed to find an installed version of gflags, repeat search allowing + # exported build directories. + message(STATUS "Failed to find installed gflags CMake configuration, " + "searching for gflags build directories exported with CMake.") + # Again pass NO_CMAKE_BUILDS_PATH, as we know that gflags is exported and + # do not want to treat projects built with the CMake GUI preferentially. + find_package(gflags QUIET + NO_MODULE + NO_CMAKE_BUILDS_PATH) + if (gflags_FOUND) + message(STATUS "Found exported gflags build directory: ${gflags_DIR}") + endif(gflags_FOUND) + endif(gflags_FOUND) + + set(FOUND_INSTALLED_GFLAGS_CMAKE_CONFIGURATION ${gflags_FOUND}) + + # gflags v2.1 - 2.1.2 shipped with a bug in their gflags-config.cmake [1] + # whereby gflags_LIBRARIES = "gflags", but there was no imported target + # called "gflags", they were called: gflags[_nothreads]-[static/shared]. + # As this causes linker errors when gflags is not installed in a location + # on the current library paths, detect if this problem is present and + # fix it. + # + # [1] https://github.com/gflags/gflags/issues/110 + if (gflags_FOUND) + # NOTE: This is not written as additional conditions in the outer + # if (gflags_FOUND) as the NOT TARGET "${gflags_LIBRARIES}" + # condition causes problems if gflags is not found. + if (${gflags_VERSION} VERSION_LESS 2.1.3 AND + NOT TARGET "${gflags_LIBRARIES}") + message(STATUS "Detected broken gflags install in: ${gflags_DIR}, " + "version: ${gflags_VERSION} <= 2.1.2 which defines gflags_LIBRARIES = " + "${gflags_LIBRARIES} which is not an imported CMake target, see: " + "https://github.com/gflags/gflags/issues/110. Attempting to fix by " + "detecting correct gflags target.") + # Ordering here expresses preference for detection, specifically we do not + # want to use the _nothreads variants if the full library is available. + list(APPEND CHECK_GFLAGS_IMPORTED_TARGET_NAMES + gflags-shared gflags-static + gflags_nothreads-shared gflags_nothreads-static) + foreach(CHECK_GFLAGS_TARGET ${CHECK_GFLAGS_IMPORTED_TARGET_NAMES}) + if (TARGET ${CHECK_GFLAGS_TARGET}) + message(STATUS "Found valid gflags target: ${CHECK_GFLAGS_TARGET}, " + "updating gflags_LIBRARIES.") + set(gflags_LIBRARIES ${CHECK_GFLAGS_TARGET}) + break() + endif() + endforeach() + if (NOT TARGET ${gflags_LIBRARIES}) + message(STATUS "Failed to fix detected broken gflags install in: " + "${gflags_DIR}, version: ${gflags_VERSION} <= 2.1.2, none of the " + "imported targets for gflags: ${CHECK_GFLAGS_IMPORTED_TARGET_NAMES} " + "are defined. Will continue with a manual search for gflags " + "components. We recommend you build/install a version of gflags > " + "2.1.2 (or master).") + set(FOUND_INSTALLED_GFLAGS_CMAKE_CONFIGURATION FALSE) + endif() + endif() + endif() + + if (FOUND_INSTALLED_GFLAGS_CMAKE_CONFIGURATION) + message(STATUS "Detected gflags version: ${gflags_VERSION}") + set(GFLAGS_FOUND ${gflags_FOUND}) + set(GFLAGS_INCLUDE_DIR ${gflags_INCLUDE_DIR}) + set(GFLAGS_LIBRARY ${gflags_LIBRARIES}) + + # gflags does not export the namespace in their CMake configuration, so + # use our function to determine what it should be, as it can be either + # gflags or google dependent upon version & configuration. + # + # NOTE: We use the regex method to determine the namespace here, as + # check_cxx_source_compiles() will not use imported targets, which + # is what gflags will be in this case. + gflags_check_gflags_namespace_using_regex() + + if (NOT GFLAGS_NAMESPACE) + gflags_report_not_found( + "Failed to determine gflags namespace using regex for gflags " + "version: ${gflags_VERSION} exported here: ${gflags_DIR} using CMake.") + endif (NOT GFLAGS_NAMESPACE) + else (FOUND_INSTALLED_GFLAGS_CMAKE_CONFIGURATION) + message(STATUS "Failed to find an installed/exported CMake configuration " + "for gflags, will perform search for installed gflags components.") + endif (FOUND_INSTALLED_GFLAGS_CMAKE_CONFIGURATION) +endif(GFLAGS_PREFER_EXPORTED_GFLAGS_CMAKE_CONFIGURATION) + +if (NOT GFLAGS_FOUND) + # Either failed to find an exported gflags CMake configuration, or user + # told us not to use one. Perform a manual search for all gflags components. + + # Handle possible presence of lib prefix for libraries on MSVC, see + # also GFLAGS_RESET_FIND_LIBRARY_PREFIX(). + if (MSVC) + # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES + # s/t we can set it back before returning. + set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") + # The empty string in this list is important, it represents the case when + # the libraries have no prefix (shared libraries / DLLs). + set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") + endif (MSVC) + + # Search user-installed locations first, so that we prefer user installs + # to system installs where both exist. + list(APPEND GFLAGS_CHECK_INCLUDE_DIRS + /usr/local/include + /usr/local/homebrew/include # Mac OS X + /opt/local/var/macports/software # Mac OS X. + /opt/local/include + /usr/include) + list(APPEND GFLAGS_CHECK_PATH_SUFFIXES + gflags/include # Windows (for C:/Program Files prefix). + gflags/Include ) # Windows (for C:/Program Files prefix). + + list(APPEND GFLAGS_CHECK_LIBRARY_DIRS + /usr/local/lib + /usr/local/homebrew/lib # Mac OS X. + /opt/local/lib + /usr/lib) + list(APPEND GFLAGS_CHECK_LIBRARY_SUFFIXES + gflags/lib # Windows (for C:/Program Files prefix). + gflags/Lib ) # Windows (for C:/Program Files prefix). + + # Search supplied hint directories first if supplied. + find_path(GFLAGS_INCLUDE_DIR + NAMES gflags/gflags.h + HINTS ${GFLAGS_INCLUDE_DIR_HINTS} + PATHS ${GFLAGS_CHECK_INCLUDE_DIRS} + PATH_SUFFIXES ${GFLAGS_CHECK_PATH_SUFFIXES}) + if (NOT GFLAGS_INCLUDE_DIR OR + NOT EXISTS ${GFLAGS_INCLUDE_DIR}) + gflags_report_not_found( + "Could not find gflags include directory, set GFLAGS_INCLUDE_DIR " + "to directory containing gflags/gflags.h") + endif (NOT GFLAGS_INCLUDE_DIR OR + NOT EXISTS ${GFLAGS_INCLUDE_DIR}) + + find_library(GFLAGS_LIBRARY NAMES gflags + HINTS ${GFLAGS_LIBRARY_DIR_HINTS} + PATHS ${GFLAGS_CHECK_LIBRARY_DIRS} + PATH_SUFFIXES ${GFLAGS_CHECK_LIBRARY_SUFFIXES}) + if (NOT GFLAGS_LIBRARY OR + NOT EXISTS ${GFLAGS_LIBRARY}) + gflags_report_not_found( + "Could not find gflags library, set GFLAGS_LIBRARY " + "to full path to libgflags.") + endif (NOT GFLAGS_LIBRARY OR + NOT EXISTS ${GFLAGS_LIBRARY}) + + # gflags typically requires a threading library (which is OS dependent), note + # that this defines the CMAKE_THREAD_LIBS_INIT variable. If we are able to + # detect threads, we assume that gflags requires it. + find_package(Threads QUIET) + set(GFLAGS_LINK_LIBRARIES ${CMAKE_THREAD_LIBS_INIT}) + # On Windows (including MinGW), the Shlwapi library is used by gflags if + # available. + if (WIN32) + include(CheckIncludeFileCXX) + check_include_file_cxx("shlwapi.h" HAVE_SHLWAPI) + if (HAVE_SHLWAPI) + list(APPEND GFLAGS_LINK_LIBRARIES shlwapi.lib) + endif(HAVE_SHLWAPI) + endif (WIN32) + + # Mark internally as found, then verify. GFLAGS_REPORT_NOT_FOUND() unsets + # if called. + set(GFLAGS_FOUND TRUE) + + # Identify what namespace gflags was built with. + if (GFLAGS_INCLUDE_DIR AND NOT GFLAGS_NAMESPACE) + # To handle Windows peculiarities / CMake bugs on MSVC we try two approaches + # to detect the gflags namespace: + # + # 1) Try to use check_cxx_source_compiles() to compile a trivial program + # with the two choices for the gflags namespace. + # + # 2) [In the event 1) fails] Use regex on the gflags headers to try to + # determine the gflags namespace. Whilst this is less robust than 1), + # it does avoid any interaction with msbuild. + gflags_check_gflags_namespace_using_try_compile() + + if (NOT GFLAGS_NAMESPACE) + # Failed to determine gflags namespace using check_cxx_source_compiles() + # method, try and obtain it using regex on the gflags headers instead. + message(STATUS "Failed to find gflags namespace using using " + "check_cxx_source_compiles(), trying namespace regex instead, " + "this is expected on Windows.") + gflags_check_gflags_namespace_using_regex() + + if (NOT GFLAGS_NAMESPACE) + gflags_report_not_found( + "Failed to determine gflags namespace either by " + "check_cxx_source_compiles(), or namespace regex.") + endif (NOT GFLAGS_NAMESPACE) + endif (NOT GFLAGS_NAMESPACE) + endif (GFLAGS_INCLUDE_DIR AND NOT GFLAGS_NAMESPACE) + + # Make the GFLAGS_NAMESPACE a cache variable s/t the user can view it, and could + # overwrite it in the CMake GUI. + set(GFLAGS_NAMESPACE "${GFLAGS_NAMESPACE}" CACHE STRING + "gflags namespace (google or gflags)" FORCE) + + # gflags does not seem to provide any record of the version in its + # source tree, thus cannot extract version. + + # Catch case when caller has set GFLAGS_NAMESPACE in the cache / GUI + # with an invalid value. + if (GFLAGS_NAMESPACE AND + NOT GFLAGS_NAMESPACE STREQUAL "google" AND + NOT GFLAGS_NAMESPACE STREQUAL "gflags") + gflags_report_not_found( + "Caller defined GFLAGS_NAMESPACE:" + " ${GFLAGS_NAMESPACE} is not valid, not google or gflags.") + endif () + # Catch case when caller has set GFLAGS_INCLUDE_DIR in the cache / GUI and + # thus FIND_[PATH/LIBRARY] are not called, but specified locations are + # invalid, otherwise we would report the library as found. + if (GFLAGS_INCLUDE_DIR AND + NOT EXISTS ${GFLAGS_INCLUDE_DIR}/gflags/gflags.h) + gflags_report_not_found( + "Caller defined GFLAGS_INCLUDE_DIR:" + " ${GFLAGS_INCLUDE_DIR} does not contain gflags/gflags.h header.") + endif (GFLAGS_INCLUDE_DIR AND + NOT EXISTS ${GFLAGS_INCLUDE_DIR}/gflags/gflags.h) + # TODO: This regex for gflags library is pretty primitive, we use lowercase + # for comparison to handle Windows using CamelCase library names, could + # this check be better? + string(TOLOWER "${GFLAGS_LIBRARY}" LOWERCASE_GFLAGS_LIBRARY) + if (GFLAGS_LIBRARY AND + NOT "${LOWERCASE_GFLAGS_LIBRARY}" MATCHES ".*gflags[^/]*") + gflags_report_not_found( + "Caller defined GFLAGS_LIBRARY: " + "${GFLAGS_LIBRARY} does not match gflags.") + endif (GFLAGS_LIBRARY AND + NOT "${LOWERCASE_GFLAGS_LIBRARY}" MATCHES ".*gflags[^/]*") + + gflags_reset_find_library_prefix() + +endif(NOT GFLAGS_FOUND) + +# Set standard CMake FindPackage variables if found. +if (GFLAGS_FOUND) + set(GFLAGS_INCLUDE_DIRS ${GFLAGS_INCLUDE_DIR}) + set(GFLAGS_LIBRARIES ${GFLAGS_LIBRARY} ${GFLAGS_LINK_LIBRARIES}) +endif (GFLAGS_FOUND) + +# Handle REQUIRED / QUIET optional arguments. +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Gflags DEFAULT_MSG + GFLAGS_INCLUDE_DIRS GFLAGS_LIBRARIES GFLAGS_NAMESPACE) + +# Only mark internal variables as advanced if we found gflags, otherwise +# leave them visible in the standard GUI for the user to set manually. +if (GFLAGS_FOUND) + mark_as_advanced(FORCE GFLAGS_INCLUDE_DIR + GFLAGS_LIBRARY + GFLAGS_NAMESPACE + gflags_DIR) # Autogenerated by find_package(gflags) +endif (GFLAGS_FOUND) diff --git a/examples/ros2/smb_estimator_graph_ros2/FindGlog.cmake b/examples/ros2/smb_estimator_graph_ros2/FindGlog.cmake new file mode 100644 index 00000000..979dceda --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/FindGlog.cmake @@ -0,0 +1,346 @@ +# Ceres Solver - A fast non-linear least squares minimizer +# Copyright 2015 Google Inc. All rights reserved. +# http://ceres-solver.org/ +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Google Inc. nor the names of its contributors may be +# used to endorse or promote products derived from this software without +# specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: alexs.mac@gmail.com (Alex Stewart) +# + +# FindGlog.cmake - Find Google glog logging library. +# +# This module defines the following variables: +# +# GLOG_FOUND: TRUE iff glog is found. +# GLOG_INCLUDE_DIRS: Include directories for glog. +# GLOG_LIBRARIES: Libraries required to link glog. +# FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION: True iff the version of glog found +# was built & installed / exported +# as a CMake package. +# +# The following variables control the behaviour of this module: +# +# GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION: TRUE/FALSE, iff TRUE then +# then prefer using an exported CMake configuration +# generated by glog > 0.3.4 over searching for the +# glog components manually. Otherwise (FALSE) +# ignore any exported glog CMake configurations and +# always perform a manual search for the components. +# Default: TRUE iff user does not define this variable +# before we are called, and does NOT specify either +# GLOG_INCLUDE_DIR_HINTS or GLOG_LIBRARY_DIR_HINTS +# otherwise FALSE. +# GLOG_INCLUDE_DIR_HINTS: List of additional directories in which to +# search for glog includes, e.g: /timbuktu/include. +# GLOG_LIBRARY_DIR_HINTS: List of additional directories in which to +# search for glog libraries, e.g: /timbuktu/lib. +# +# The following variables are also defined by this module, but in line with +# CMake recommended FindPackage() module style should NOT be referenced directly +# by callers (use the plural variables detailed above instead). These variables +# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which +# are NOT re-called (i.e. search for library is not repeated) if these variables +# are set with valid values _in the CMake cache_. This means that if these +# variables are set directly in the cache, either by the user in the CMake GUI, +# or by the user passing -DVAR=VALUE directives to CMake when called (which +# explicitly defines a cache variable), then they will be used verbatim, +# bypassing the HINTS variables and other hard-coded search locations. +# +# GLOG_INCLUDE_DIR: Include directory for glog, not including the +# include directory of any dependencies. +# GLOG_LIBRARY: glog library, not including the libraries of any +# dependencies. + +# Reset CALLERS_CMAKE_FIND_LIBRARY_PREFIXES to its value when +# FindGlog was invoked. +macro(GLOG_RESET_FIND_LIBRARY_PREFIX) + if (MSVC AND CALLERS_CMAKE_FIND_LIBRARY_PREFIXES) + set(CMAKE_FIND_LIBRARY_PREFIXES "${CALLERS_CMAKE_FIND_LIBRARY_PREFIXES}") + endif() +endmacro(GLOG_RESET_FIND_LIBRARY_PREFIX) + +# Called if we failed to find glog or any of it's required dependencies, +# unsets all public (designed to be used externally) variables and reports +# error message at priority depending upon [REQUIRED/QUIET/] argument. +macro(GLOG_REPORT_NOT_FOUND REASON_MSG) + unset(GLOG_FOUND) + unset(GLOG_INCLUDE_DIRS) + unset(GLOG_LIBRARIES) + # Make results of search visible in the CMake GUI if glog has not + # been found so that user does not have to toggle to advanced view. + mark_as_advanced(CLEAR GLOG_INCLUDE_DIR + GLOG_LIBRARY) + + glog_reset_find_library_prefix() + + # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() + # use the camelcase library name, not uppercase. + if (Glog_FIND_QUIETLY) + message(STATUS "Failed to find glog - " ${REASON_MSG} ${ARGN}) + elseif (Glog_FIND_REQUIRED) + message(FATAL_ERROR "Failed to find glog - " ${REASON_MSG} ${ARGN}) + else() + # Neither QUIETLY nor REQUIRED, use no priority which emits a message + # but continues configuration and allows generation. + message("-- Failed to find glog - " ${REASON_MSG} ${ARGN}) + endif () + return() +endmacro(GLOG_REPORT_NOT_FOUND) + +# Protect against any alternative find_package scripts for this library having +# been called previously (in a client project) which set GLOG_FOUND, but not +# the other variables we require / set here which could cause the search logic +# here to fail. +unset(GLOG_FOUND) + +# ----------------------------------------------------------------- +# By default, if the user has expressed no preference for using an exported +# glog CMake configuration over performing a search for the installed +# components, and has not specified any hints for the search locations, then +# prefer a glog exported configuration if available. +if (NOT DEFINED GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION + AND NOT GLOG_INCLUDE_DIR_HINTS + AND NOT GLOG_LIBRARY_DIR_HINTS) + message(STATUS "No preference for use of exported glog CMake configuration " + "set, and no hints for include/library directories provided. " + "Defaulting to preferring an installed/exported glog CMake configuration " + "if available.") + set(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION TRUE) +endif() + +if (GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION) + # Try to find an exported CMake configuration for glog, as generated by + # glog versions > 0.3.4 + # + # We search twice, s/t we can invert the ordering of precedence used by + # find_package() for exported package build directories, and installed + # packages (found via CMAKE_SYSTEM_PREFIX_PATH), listed as items 6) and 7) + # respectively in [1]. + # + # By default, exported build directories are (in theory) detected first, and + # this is usually the case on Windows. However, on OS X & Linux, the install + # path (/usr/local) is typically present in the PATH environment variable + # which is checked in item 4) in [1] (i.e. before both of the above, unless + # NO_SYSTEM_ENVIRONMENT_PATH is passed). As such on those OSs installed + # packages are usually detected in preference to exported package build + # directories. + # + # To ensure a more consistent response across all OSs, and as users usually + # want to prefer an installed version of a package over a locally built one + # where both exist (esp. as the exported build directory might be removed + # after installation), we first search with NO_CMAKE_PACKAGE_REGISTRY which + # means any build directories exported by the user are ignored, and thus + # installed directories are preferred. If this fails to find the package + # we then research again, but without NO_CMAKE_PACKAGE_REGISTRY, so any + # exported build directories will now be detected. + # + # To prevent confusion on Windows, we also pass NO_CMAKE_BUILDS_PATH (which + # is item 5) in [1]), to not preferentially use projects that were built + # recently with the CMake GUI to ensure that we always prefer an installed + # version if available. + # + # NOTE: We use the NAMES option as glog erroneously uses 'google-glog' as its + # project name when built with CMake, but exports itself as just 'glog'. + # On Linux/OS X this does not break detection as the project name is + # not used as part of the install path for the CMake package files, + # e.g. /usr/local/lib/cmake/glog, where the suffix is hardcoded + # in glog's CMakeLists. However, on Windows the project name *is* + # part of the install prefix: C:/Program Files/google-glog/[include,lib]. + # However, by default CMake checks: + # C:/Program Files/ which does not + # exist and thus detection fails. Thus we use the NAMES to force the + # search to use both google-glog & glog. + # + # [1] http://www.cmake.org/cmake/help/v2.8.11/cmake.html#command:find_package + find_package(glog QUIET + NAMES google-glog glog + NO_MODULE + NO_CMAKE_PACKAGE_REGISTRY + NO_CMAKE_BUILDS_PATH) + if (glog_FOUND) + message(STATUS "Found installed version of glog: ${glog_DIR}") + else() + # Failed to find an installed version of glog, repeat search allowing + # exported build directories. + message(STATUS "Failed to find installed glog CMake configuration, " + "searching for glog build directories exported with CMake.") + # Again pass NO_CMAKE_BUILDS_PATH, as we know that glog is exported and + # do not want to treat projects built with the CMake GUI preferentially. + find_package(glog QUIET + NAMES google-glog glog + NO_MODULE + NO_CMAKE_BUILDS_PATH) + if (glog_FOUND) + message(STATUS "Found exported glog build directory: ${glog_DIR}") + endif(glog_FOUND) + endif(glog_FOUND) + + set(FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION ${glog_FOUND}) + + if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) + message(STATUS "Detected glog version: ${glog_VERSION}") + set(GLOG_FOUND ${glog_FOUND}) + # glog wraps the include directories into the exported glog::glog target. + set(GLOG_INCLUDE_DIR "") + set(GLOG_LIBRARY glog::glog) + else (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) + message(STATUS "Failed to find an installed/exported CMake configuration " + "for glog, will perform search for installed glog components.") + endif (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) +endif(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION) + +if (NOT GLOG_FOUND) + # Either failed to find an exported glog CMake configuration, or user + # told us not to use one. Perform a manual search for all glog components. + + # Handle possible presence of lib prefix for libraries on MSVC, see + # also GLOG_RESET_FIND_LIBRARY_PREFIX(). + if (MSVC) + # Preserve the caller's original values for CMAKE_FIND_LIBRARY_PREFIXES + # s/t we can set it back before returning. + set(CALLERS_CMAKE_FIND_LIBRARY_PREFIXES "${CMAKE_FIND_LIBRARY_PREFIXES}") + # The empty string in this list is important, it represents the case when + # the libraries have no prefix (shared libraries / DLLs). + set(CMAKE_FIND_LIBRARY_PREFIXES "lib" "" "${CMAKE_FIND_LIBRARY_PREFIXES}") + endif (MSVC) + + # Search user-installed locations first, so that we prefer user installs + # to system installs where both exist. + list(APPEND GLOG_CHECK_INCLUDE_DIRS + /usr/local/include + /usr/local/homebrew/include # Mac OS X + /opt/local/var/macports/software # Mac OS X. + /opt/local/include + /usr/include) + # Windows (for C:/Program Files prefix). + list(APPEND GLOG_CHECK_PATH_SUFFIXES + glog/include + glog/Include + Glog/include + Glog/Include + google-glog/include # CMake installs with project name prefix. + google-glog/Include) + + list(APPEND GLOG_CHECK_LIBRARY_DIRS + /usr/local/lib + /usr/local/homebrew/lib # Mac OS X. + /opt/local/lib + /usr/lib) + # Windows (for C:/Program Files prefix). + list(APPEND GLOG_CHECK_LIBRARY_SUFFIXES + glog/lib + glog/Lib + Glog/lib + Glog/Lib + google-glog/lib # CMake installs with project name prefix. + google-glog/Lib) + + # Search supplied hint directories first if supplied. + find_path(GLOG_INCLUDE_DIR + NAMES glog/logging.h + HINTS ${GLOG_INCLUDE_DIR_HINTS} + PATHS ${GLOG_CHECK_INCLUDE_DIRS} + PATH_SUFFIXES ${GLOG_CHECK_PATH_SUFFIXES}) + if (NOT GLOG_INCLUDE_DIR OR + NOT EXISTS ${GLOG_INCLUDE_DIR}) + glog_report_not_found( + "Could not find glog include directory, set GLOG_INCLUDE_DIR " + "to directory containing glog/logging.h") + endif (NOT GLOG_INCLUDE_DIR OR + NOT EXISTS ${GLOG_INCLUDE_DIR}) + + find_library(GLOG_LIBRARY NAMES glog + HINTS ${GLOG_LIBRARY_DIR_HINTS} + PATHS ${GLOG_CHECK_LIBRARY_DIRS} + PATH_SUFFIXES ${GLOG_CHECK_LIBRARY_SUFFIXES}) + if (NOT GLOG_LIBRARY OR + NOT EXISTS ${GLOG_LIBRARY}) + glog_report_not_found( + "Could not find glog library, set GLOG_LIBRARY " + "to full path to libglog.") + endif (NOT GLOG_LIBRARY OR + NOT EXISTS ${GLOG_LIBRARY}) + + # Mark internally as found, then verify. GLOG_REPORT_NOT_FOUND() unsets + # if called. + set(GLOG_FOUND TRUE) + + # Glog does not seem to provide any record of the version in its + # source tree, thus cannot extract version. + + # Catch case when caller has set GLOG_INCLUDE_DIR in the cache / GUI and + # thus FIND_[PATH/LIBRARY] are not called, but specified locations are + # invalid, otherwise we would report the library as found. + if (GLOG_INCLUDE_DIR AND + NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) + glog_report_not_found( + "Caller defined GLOG_INCLUDE_DIR:" + " ${GLOG_INCLUDE_DIR} does not contain glog/logging.h header.") + endif (GLOG_INCLUDE_DIR AND + NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) + # TODO: This regex for glog library is pretty primitive, we use lowercase + # for comparison to handle Windows using CamelCase library names, could + # this check be better? + string(TOLOWER "${GLOG_LIBRARY}" LOWERCASE_GLOG_LIBRARY) + if (GLOG_LIBRARY AND + NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*") + glog_report_not_found( + "Caller defined GLOG_LIBRARY: " + "${GLOG_LIBRARY} does not match glog.") + endif (GLOG_LIBRARY AND + NOT "${LOWERCASE_GLOG_LIBRARY}" MATCHES ".*glog[^/]*") + + glog_reset_find_library_prefix() + +endif(NOT GLOG_FOUND) + +# Set standard CMake FindPackage variables if found. +if (GLOG_FOUND) + set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) + set(GLOG_LIBRARIES ${GLOG_LIBRARY}) +endif (GLOG_FOUND) + +# If we are using an exported CMake glog target, the include directories are +# wrapped into the target itself, and do not have to be (and are not) +# separately specified. In which case, we should not add GLOG_INCLUDE_DIRS +# to the list of required variables in order that glog be reported as found. +if (FOUND_INSTALLED_GLOG_CMAKE_CONFIGURATION) + set(GLOG_REQUIRED_VARIABLES GLOG_LIBRARIES) +else() + set(GLOG_REQUIRED_VARIABLES GLOG_INCLUDE_DIRS GLOG_LIBRARIES) +endif() + +# Handle REQUIRED / QUIET optional arguments. +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Glog DEFAULT_MSG + ${GLOG_REQUIRED_VARIABLES}) + +# Only mark internal variables as advanced if we found glog, otherwise +# leave them visible in the standard GUI for the user to set manually. +if (GLOG_FOUND) + mark_as_advanced(FORCE GLOG_INCLUDE_DIR + GLOG_LIBRARY + glog_DIR) # Autogenerated by find_package(glog) +endif (GLOG_FOUND) diff --git a/examples/ros2/smb_estimator_graph_ros2/config/compslam/compslam_lio_RS16_params.yaml b/examples/ros2/smb_estimator_graph_ros2/config/compslam/compslam_lio_RS16_params.yaml new file mode 100644 index 00000000..3bfdebc8 --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/config/compslam/compslam_lio_RS16_params.yaml @@ -0,0 +1,109 @@ +#Common +loamVerbosity: 0 #Debug Output, 0-disable, higher for more verbose output +scanPeriod: 0.1 #Expected scan period for input pointcloud in seconds. Used of Distortion correction +ioRatio: 2 #Ratio of publish rate of LaserOdometry w.r.t input PCL rate. LO is calculated for each PCL but published at slower rate to Mapping, default 2 +lidarFrame: rslidar #LiDAR frame name - used for LiDAR-to-Sensor transform lookup +rotateInputCloud: false #Flag to rotate input cloud before estimating odometry/map so produced resuts are ROS frame aligned irrespective of LiDAR mounting orientation +inputCloudRotation: [0.0, 0.0, 0.0] #Rotation applied to input cloud - ORDER YPR(radians) #90 deg = 1.5707963268 rad + +#External Prior/Transform Input +forceExtPriorUseMapping : false #Flag to force use of FULL external prior instead of LaserOdometry TRANSLATION Only Estimate +extPriorAvailable: true #Flag to check if 'Primary' external prior is available +extOdomFrame: imu #External Prior odometry frame name +extFixedFrame: odom #External Prior fixed frame name +extSensorFrame: imu_link #External Prior sensor frame name +extOdomTimeOffset : 0.0 #Timeoffset (seconds) between LiDAR pointcloud and external source +fallbackExtPriorAvailable: false #Flag to check if 'Fallback ' external prior is available +fallbackExtOdomFrame: vio_imu #Fallback External Prior odometry frame name +fallbackExtFixedFrame: vio_imu_init #Fallback External Prior fixed frame name +fallbackExtSensorFrame: imu_sensor_frame #Fallback External Prior sensor frame name +fallbackExtOdomTimeOffset: 0.00 #Timeoffset (seconds) between LiDAR pointcloud and Fallbackexternal source + +#MultiScanRegistration +lidar: VLP-16 #Choose LiDAR type - options: VLP-16 HDL-32 HDL-64E O1-16 O1-64 Bperl-32 +useCloudIntensityandRingFields : false #Flag to use input pointcloud intensity or ring fields. Converts to custom PointXYZIR instead of PointXYZ. True for Bpearl +uniformLidarRingDistance: true # Whether the LiDAR rings are uniformly distributed in elevation, does not hold for e.g. Helios +ringAngleTolerance: 0.5 # [deg], Tolerance up to which angle rays are considered to belong to same ring, only impact if argument before is false +imuHistorySize: 800 #IMU Message Buffer Size , default: 200 +minRange: 0.1 #Minimum Range of useful points, default: 0.1 +maxRange: 80.0 #Maximum Range of useful points, default: 80 +featureRegions: 6 #Number of Azimuth Regions Pointcloud is divided, default:6 +curvatureRegion: 5 #Number of neigboring points on a scan line on each side of a point used for calculating curvature of that point, default 5 +maxCornerSharp: 2 #Number of Sharp Features per scan line in each curvatureRegion, default:2 +maxCornerLessSharp: 20 #Number of Less Sharp Features per scan line in each curvatureRegion, default:10*maxCornerLessSharp +maxSurfaceFlat: 4 #Number of FlatFeatures per scan line in each curvatureRegion, default:4 +surfaceCurvatureThreshold: 0.1 #Threshold above which feature is categorized Sharp, default 0.1 +lessFlatFilterSize: 0.2 #Leaf Size for downsampling remaing pointcloud after feature selection, default 0.2 +checkInvalidFeatureRegions: false +publishDebugArrowsToRemovedFeatures: false +azimuthBoundaries: [1.1,1.5,1.7,2.2] #ANYmal + +#LaserOdometry +undistortInputCloud: true #If true External Prior or Motion Model will be used for LiDAR Ego Motion Compensation of input cloud +odomMaxIterations: 25 #Maximum Number of LO optimization iterations, default 25 +odomMinIterations: 1 #Minimum Number of LO optimization iterations, default 4, (set 0 to perform only first iteration at minimum) +odomDeltaTAbort: 0.05 #Translation threshold for optimization convergence, deafult 0.1 (m) +odomDeltaRAbort: 0.05 #Rotation threshold for optimization convergence, deafult 0.1 (deg) +odomDegenEigVal: 30 #Minimum eignevalue threshold for determining degeneracy of LO optimization, default 30 +odomRelativeTranslationMax: 0.8 #Max translation threshold between two pointclouds for external odometry to be considered valid input. Determined w.r.t set max robot movement speed +odomRelativeRotationMax: 0.1 #Max rotation threshold between two pointclouds from external odometry to be considered valid input. Determined w.r.t set max robot movement speed + +#LaserMapping +mapMaxIterations: 10 #Maximum Number of LM optimization iterations, default 10 +mapMinIterations: 1 #Minimum Number of LM optimization iterations, default 1, (set 0 to perform only first iteration at minimum) +mapDeltaTAbort: 0.05 #Translation threshold for optimization convergence, deafult 0.05 (m) +mapDeltaRAbort: 0.05 #Rotation threshold for optimization convergence, deafult 0.05 (deg) +cornerFilterSize: 0.2 #Leaf Size for downsampling current CORNER pointcloud before merging in map, default 0.2 +surfaceFilterSize: 0.2 #Leaf Size for downsampling current FLAT/SURFACE pointcloud before merging in map, default 0.2 +mapVisFilterSize: 0.4 #Leaf Size for downsampling current visualization Map pointcloud, default 0.5 +rndSampleMapVisNoOfSamples: 100000 #Number of Samples for random sampling of the visualized map cloud, default 100000 +mapCubeSize: 10.0 #Size of Cube/Voxel used for saving internal map (meters), default 50 +mapDimensionsInCubes: [101,21,101] #WxHxD of internal map in CUBE UNITS, default:[21,11,21] #Width,Height,Depth +mapStartLocationInCubes: [50,10,50] #Robot start position in internal map in CUBE UNITS, default:[10,5,10] #Width,Height,Depth +numNeighborSubmapCubes: 4 #Number of Neigboring cubes in +/- direction along each axis to build submap for matching +mapDegenEigVal: 10 #Minimum eignevalue threshold for determining degeneracy of LM optimization, default 30 +mapPriorNormThresh: 0.5 #Max translation threshold between two pointclouds from external odometry to be considered valid input. Determined w.r.t set max robot movement speed +useSavedSubmapsForInitialization: false #Use saved submap on disc for initialization +submapLocalizationInitGuess: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #Initialization guess of robot location in submap - ORDER: XYZ(meters) - YPR(radians) +waitAndUseExternalSubmapsForInit: false #Wait for external submaps before initlization. Trigger external submaps through service call +submapsRequestServiceCall: "" #Name of service call to request submaps from other node (xyz/share_submaps) +useExternalSubmapOriginForInit: false #Use origin of external submaps as initlization guess for co-localization instead of current external pose (default: false) +forcePathPublishing: true #Force ROS path message publishing for mapping odometry +mapPublishRateDivider: 20 #Publishes full map at 1.0/(scanPeriod * ioRatio * mapPublishRateDivider), default: 20(0.25 Hz) +useExtRotationAlignment: true #Use external fixed & map frame to detemine gravity misalignment, default: true +submapVisCropRange: 0 #Range of cropping for submap visualization (set 0 to disable) + +#Factor Graph +imuFrame: imu_link #Frame of IMU used in integrator - Used for lookup transform with lidarFrame +imuRate: 400 #Rate of IMU input (Hz) used for initialization of gravity, initial Roll/Pitch and gyro bias +imuTimeForInit: 0.5 #Time interval of IMU measurements used for initialization +imuTimeOffset: 0.0 #seconds #Offset between IMU and LiDAR Measurements - Depending on LiDAR timestamp first(+0.05) or last(-0.05) +initGraphRollPitchFromIMU: true #Initialize graph roll/pitch from IMU +globalZaxisPointsUp: true #Z-axis of global coordinate system pointing up +accNoiseDensity: 7.84e-04 #Continuous-time "Covariance" of accelerometer +accBiasRandomWalk: 1.0e-04 #Continuous-time "Covariance" describing accelerometer bias random walk (biasAccCovariance) +gyrNoiseDensity: 3.4906585e-5 #Continuous-time "Covariance" of gyroscope measurements +gyrBiasRandomWalk: 1.0e-04 #Continuous-time "Covariance" describing gyroscope bias random walk (biasOmegaCovariance) +imuIntegrationCovariance: 1.0e-08 #Continuous-time "Covariance" describing integration uncertainty +imuBiasAccOmegaInt: 1.0 #Covariance of bias used for pre-integration +accBiasPrior: [0.0, 0.0, 0.0] #Prior/starting value of accelerometer bias +gyrBiasPrior: [0.0, 0.0, 0.0] #Prior/starting value of gyroscope bias +smootherLag: 3.0 #Lag of fixed lag smoother[seconds] +additonalIterations: 3 #Additional iterations of graph optimizer after update with new factors +positionReLinTh: 0.05 #Position linearization threshold +rotationReLinTh: 0.05 #Rotation linearization threshold +velocityReLinTh: 0.1 #Linear Velocity linearization threshold +accBiasReLinTh: 0.1 #Accelerometer bias linearization threshold +gyrBiasReLinTh: 0.1 #Gyroscope bias linearization threshold +relinearizeSkip: 1 +enableRelinearization: true +evaluateNonlinearError: false +cacheLinearizedFactors: true +findUnusedFactorSlots: false +enablePartialRelinearizationCheck: true +enableDetailedResults: false +poseBetweenNoise: [0.5, 0.5, 0.05, 0.1, 0.1, 0.1] #Noise of add between factor -ORDER RPY(rad) - XYZ(meters) +zeroMotionDetection: false #Detect and Add Zero Motion Factors(Zero delta Pose and Velocity) +zeroMotionThreshold: 0.01 #Zero motion threshold in meters (Currently only motion detection is translation only) +minZeroMoitionDetections: 10 #Minimum number of consective zero motions detected before factors are added +gravityRollPitchFactors: false #Add Gravity-aligned Roll-Pitch from IMU when in Zero motion (only works if Zero-Motion Factors are added) diff --git a/examples/ros2/smb_estimator_graph_ros2/config/core/core_extrinsic_params.yaml b/examples/ros2/smb_estimator_graph_ros2/config/core/core_extrinsic_params.yaml new file mode 100644 index 00000000..669519d2 --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/config/core/core_extrinsic_params.yaml @@ -0,0 +1,14 @@ +#External Prior/Transform Input +extrinsics: + # Published by GMSF + worldFrame: "world_graph_msf" + odomFrame: "odom_graph_msf" + # Used for estimation + imuFrame: "imu" + baseLinkFrame: "base_link" + initializeZeroYawAndPositionOfFrame: "rslidar" # Initialize the yaw and position of the base frame to zero + # this is just meant to avoid any jump after getting the first true measurements + +name_ids: + referenceFrameAligned: "_graph_msf_aligned" # Specifies what is the suffix of the aligned published fixed frames + sensorFrameCorrected: "_graph_msf_corrected" # Specifies what is the suffix of the corrected published sensor frames diff --git a/examples/ros2/smb_estimator_graph_ros2/config/core/core_graph_config.yaml b/examples/ros2/smb_estimator_graph_ros2/config/core/core_graph_config.yaml new file mode 100644 index 00000000..b25fd342 --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/config/core/core_graph_config.yaml @@ -0,0 +1,6 @@ +#Common +common_params: + verbosity: 0 #Debug Output, 0: only important events, 1: optimization duration, 2: added factors + odomNotJumpAtStart: false #Whether the World->Map transform should be updated in the beginning (odom start at identity) + logRealTimeStateToMemory: false # Whether the real-time state should be logged to memory (for logging purposes) + logLatencyAndUpdateDurationToMemory: false # Whether the latency and update duration should be logged to memory (for logging purposes) diff --git a/examples/ros2/smb_estimator_graph_ros2/config/core/core_graph_params.yaml b/examples/ros2/smb_estimator_graph_ros2/config/core/core_graph_params.yaml new file mode 100644 index 00000000..1d88f42d --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/config/core/core_graph_params.yaml @@ -0,0 +1,79 @@ +# Sensor Params +sensor_params: + imuRate: 200.0 # Rate of IMU input (Hz) + createStateEveryNthImuMeasurement: 4 # Create a new state every n-th IMU measurement + useImuSignalLowPassFilter: false # If true, a low pass filter is applied to the IMU measurements + imuLowPassFilterCutoffFreq: 30.0 # Cutoff frequency of the low pass filter + imuBufferLength: 800 + imuTimeOffset: 0.0 # Offset between IMU and LiDAR Measurements: can be determined with rqt_multiplot + +# Initialization +initialization_params: + estimateGravityFromImu: false # If true, the gravity is estimated in the beginning using the IMU + gravityMagnitude: 9.80600 # If estimateGravityFromImu is false, this value is used as gravity + +# Factor Graph +graph_params: + realTimeSmootherLag: 2.0 # Lag of real-time fixed lag smoother[seconds] + realTimeSmootherUseIsam: true + realTimeSmootherUseCholeskyFactorization: true # CHOLESKY faster, QR numerically more stable + useAdditionalSlowBatchSmoother: true # If true, a slower smoother is used in addition to the real-time smoother + slowBatchSmootherUseIsam: false # if false, then levenberg-marquardt is used for the slow batch smoother, NO EFFECT YET + slowBatchSmootherUseCholeskyFactorization: false # CHOLESKY faster, QR numerically more stable + # Optimizer Config + gaussNewtonWildfireThreshold: 0.001 # Threshold for the wildfire in the Gauss-Newton optimization + minOptimizationFrequency: 1.0 # Minimum optimization frequency [Hz], makes sure that optimization is triggered at least every x Hz + maxOptimizationFrequency: 10.0 # Maximum optimization frequency [Hz], can be used to control computational load + additionalOptimizationIterations: 0 # Additional iterations of graph optimizer after update with new factors + findUnusedFactorSlots: true + enableDetailedResults: false + usingBiasForPreIntegration: true # If true, the bias is used during pre-integration + useWindowForMarginalsComputation: true # If true, the window is used for marginal computation + windowSizeSecondsForMarginalsComputation: 300.0 # Size of the window for marginal computation, unit: seconds + # Alignment Parameters + optimizeReferenceFramePosesWrtWorld: true # If true, the poses of the fixed frames are optimized w.r.t. world + referenceFramePosesResetThreshold: 8.0 # Reset T_M_W if distance between initial guess and optimization are too big, [SE(3) distance] + centerMeasurementsAtKeyframePositionBeforeAlignment: true # If true, the measurements are centered before alignment + createReferenceAlignmentKeyframeEveryNSeconds: 10.0 # Create a new keyframe for alignment every n seconds + # Extrinsic Calibration + optimizeExtrinsicSensorToSensorCorrectedOffset: false # If true, the extrinsic calibration is optimized + +# Noise Parameters +noise_params: + ## IMU + ### Position + accNoiseDensity: 2.2555e-04 # Continuous-time Noise Amplitude Spectral Density (StdDev) [m/s^2/√Hz)], default=sqrt(7.84e-06) + integrationNoiseDensity: 1.0e-04 # Continuous-time Noise Amplitude Spectral Density of integration uncertainty, default: sqrt(1.0e-07) + use2ndOrderCoriolis: false # Whether to use 2nd order coriolis effect + ### Rotation + gyrNoiseDensity: 2.356e-04 # Continuous-time Noise Amplitude Spectral Density (StdDev) [rad/s/√Hz], default=sqrt(3.4906585e-07) + omegaCoriolis: 1.07e-04 # Coriolis effect, positive on northern hemisphere, 0 at the equator, default (central europe, Switzerland): 1.07e-04 + ### Bias + accBiasRandomWalkNoiseDensity: 4.33e-03 # Continuous-time Noise Amplitude Spectral Density of Accelerometer bias random walk [m/s^3/√Hz], default: sqrt(1.0e-03) + gyrBiasRandomWalkNoiseDensity: 2.66e-04 # Continuous-time Noise Amplitude Spectral Density of Gyroscope bias random walk [rad/s^2/√Hz], default: default: sqrt(9.33e-04) + biasAccOmegaInit: 1.0e-05 # StdDev of bias at start: default: sqrt(1.0e-07) + accBiasPrior: 0.0 # Prior/starting value of accelerometer bias + gyrBiasPrior: 0.0 # Prior/starting value of gyroscope bias + ## Initial State + initialPositionNoiseDensity: 1.0e-04 # Prior/starting value of position + initialOrientationNoiseDensity: 1.0e-03 # Prior/starting value of orientation + initialVelocityNoiseDensity: 1.0e-02 # Prior/starting value of velocity + initialAccBiasNoiseDensity: 1.0e-03 # Prior/starting value of accelerometer bias + initialGyroBiasNoiseDensity: 1.0e-03 # Prior/starting value of gyroscope bias + +# Relinearization +relinearization_params: + positionReLinTh: 1.0e-03 # Letter "x" in GTSAM variables, Position linearization threshold + rotationReLinTh: 1.0e-03 # Letter "x" in GTSAM variables, Rotation linearization threshold + velocityReLinTh: 1.0e-03 # Letter "v" in GTSAM variables, Linear Velocity linearization threshold + accBiasReLinTh: 1.0e-03 # Letter "b" in GTSAM variables, Accelerometer bias linearization threshold + gyrBiasReLinTh: 1.0e-03 # Letter "b" in GTSAM variables, Gyroscope bias linearization threshold + referenceFrameReLinTh: 1.0e-03 # Letter "r" in GTSAM variables, Reference frame linearization threshold, ONLY IF optimizeReferenceFramePosesWrtWorld + calibrationReLinTh: 1.0e-03 # Letter "c" in GTSAM variables, Calibration linearization threshold, ONLY IF optimizeExtrinsicSensorToSensorCorrectedOffset + displacementReLinTh: 1.0e-03 # Letter "d" in GTSAM variables, Displacement linearization threshold, ONLY IF optimizeExtrinsicSensorToSensorCorrectedOffset + landmarkReLinTh: 1.0e-03 # Letter "l" in GTSAM variables, Landmark linearization threshold + relinearizeSkip: 10 # Re-linearization is skipped every n-th step, default: 10 + enableRelinearization: true # Whether to use relinearization, default: true + evaluateNonlinearError: true # Whether to evaluate the nonlinear error before and after the update, default: false + cacheLinearizedFactors: true # Whether to cache the linearized factors, default: true + enablePartialRelinearizationCheck: false # Whether potentially only parts of the tree needs to be relinearized, default: false diff --git a/examples/ros2/smb_estimator_graph_ros2/config/dumped_params/2023-06-13-13-15-57.yaml b/examples/ros2/smb_estimator_graph_ros2/config/dumped_params/2023-06-13-13-15-57.yaml new file mode 100644 index 00000000..403a3779 --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/config/dumped_params/2023-06-13-13-15-57.yaml @@ -0,0 +1,523 @@ +control: + joint_state_controller: + publish_rate: 50 + type: joint_state_controller/JointStateController + joy_teleop: + joy_node: + autorepeat_rate: 20 + deadzone: 0.1 + dev: /dev/input/js0 + teleop_twist_joy: + axis_angular: 0 + axis_linear: 1 + enable_button: 4 + enable_turbo_button: 5 + scale_angular: 1.2 + scale_angular_turbo: 1.2 + scale_linear: 1.6 + scale_linear_turbo: 1.0 + lowlevel_controller: + LF_WHEEL_JOINT: + has_acceleration_limits: true + has_effort_limits: true + has_jerk_limits: true + has_position_limits: false + has_velocity_limits: true + max_acceleration: 5.0 + max_effort: 1.0 + max_jerk: 100.0 + max_velocity: 2.0 + LF_WHEEL_JOINT_dc_controller: + antiwindup: true + d: 0.0 + i: 150.0 + i_clamp: 300 + i_clamp_max: 300.0 + i_clamp_min: -300.0 + p: 50.0 + publish_state: true + LF_WHEEL_velocity_controller: + joint: LF_WHEEL_JOINT + pid: + d: 10.0 + i: 0.01 + p: 100.0 + type: velocity_controllers/JointVelocityController + RF_WHEEL_JOINT: + has_acceleration_limits: true + has_effort_limits: true + has_jerk_limits: true + has_position_limits: false + has_velocity_limits: true + max_acceleration: 5.0 + max_effort: 1.0 + max_jerk: 100.0 + max_velocity: 2.0 + RF_WHEEL_JOINT_dc_controller: + antiwindup: true + d: 0.0 + i: 150.0 + i_clamp: 300 + i_clamp_max: 300.0 + i_clamp_min: -300.0 + p: 50.0 + publish_state: true + RF_WHEEL_velocity_controller: + joint: RF_WHEEL_JOINT + pid: + d: 10.0 + i: 0.01 + p: 100.0 + type: velocity_controllers/JointVelocityController + WHEEL_JOINT_ff_param: + ff_general: 25 + ff_pure_rotation: 100 + robot_joint_publisher: + publish_rate: 50 + type: joint_state_controller/JointStateController + smb: + ang_vel_scale: 1.8 + hardware_interface: + joints: + - LF_WHEEL_JOINT + - RF_WHEEL_JOINT + loop_hz: 400 + lin_vel_scale: 1.8 + smb_diff_drive: + angular: + z: + has_acceleration_limits: true + has_velocity_limits: true + max_acceleration: 5.0 + max_velocity: 2.0 + base_frame_id: base_link + cmd_vel_timeout: 0.25 + enable_odom_tf: false + estimate_velocity_from_position: false + left_wheel: + - LF_WHEEL_JOINT + left_wheel_radius_multiplier: 1.0 + linear: + x: + has_acceleration_limits: true + has_velocity_limits: true + max_acceleration: 5.0 + max_velocity: 2.0 + pose_covariance_diagonal: + - 0.001 + - 0.001 + - 0.001 + - 0.001 + - 0.001 + - 0.03 + publish_rate: 50.0 + right_wheel: + - RF_WHEEL_JOINT + right_wheel_radius_multiplier: 1.0 + twist_covariance_diagonal: + - 0.001 + - 0.001 + - 0.001 + - 0.001 + - 0.001 + - 0.03 + type: diff_drive_controller/DiffDriveController + velocity_rolling_window_size: 2 + wheel_radius_multiplier: 1.0 + wheel_separation_multiplier: 1.0 + smb_lowlevel_controller: + command_smb: true + control_namespace: /control + controller_namespace: lowlevel_controller + port: /dev/ttyUSB0 + smb: + ang_vel_scale: 1.8 + hardware_interface: + joints: + - LF_WHEEL_JOINT + - RF_WHEEL_JOINT + loop_hz: 400 + lin_vel_scale: 1.8 + smb_robot_state_publisher: + publish_frequency: 50 + use_tf_static: true + twist_mux: + locks: + - name: e_stop + priority: 255 + timeout: 0.0 + topic: joy_teleop/e_stop + - name: e_stop_opc + priority: 200 + timeout: 0.0 + topic: opc/e_stop + topics: + - name: RC + priority: 15 + timeout: 0.5 + topic: smb_lowlevel_controller/rc_twist + - name: joy + priority: 10 + timeout: 0.5 + topic: joy_teleop/cmd_vel + - name: keyboard + priority: 9 + timeout: 0.5 + topic: keyboard_teleop/cmd_vel + - name: joy_opc + priority: 8 + timeout: 0.5 + topic: opc/cmd_vel + - name: mpc_command_twist + priority: 5 + timeout: 0.2 + topic: smb_mpc/command_twist + - name: teb_planner_twist + priority: 3 + timeout: 1.0 + topic: teb_planner_twist +odometry: + tracking_camera_odometry_conversion: + in_odom_frame: tracking_camera_pose_frame + in_sensor_frame: tracking_camera_pose_frame + out_odom_frame: tracking_camera_odom + out_sensor_frame: base_link +rosdistro: 'noetic + + ' +roslaunch: + uris: + host_smb_263_nuc__35755: http://smb-263-nuc:35755/ +rosserial_python: + baud: 250000 + port: /dev/versavis +rosversion: '1.16.0 + + ' +run_id: d353b820-09d9-11ee-9f1a-13a7083ee06f +serial_node: + baud: 115200 + port: /dev/smb-power +simulation: false +smb_description: "\n\n\n\n\n\n \n \n\ + \ \n \n \n \n \n \n \n \n\ + \ \n \n\ + \ \n \n \n \n \n \n\ + \ \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\ + \ \n \n \n \n \n \n\ + \ \n \n \n \n \n \n \n \n \n \ + \ \n \n \n \n \n \n \n \n \n \n \n \n \n \n\ + \ Gazebo/Grey\n \n \n Gazebo/Red\n \n \n \n \n\ + \ \n \n \n\ + \ \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \ + \ \n \n \n \n \n \n \n \n \n \n \n \n\ + \ \n \n \n \n \n \n 10000000.0\n\ + \ 1.0\n 0.005\n 100.0\n\ + \ Gazebo/DarkGrey\n \n \n transmission_interface/SimpleTransmission\n\ + \ \n hardware_interface/VelocityJointInterface\n\ + \ hardware_interface/EffortJointInterface\n\ + \ hardware_interface/PositionJointInterface\n\ + \ \n \n hardware_interface/VelocityJointInterface\n\ + \ hardware_interface/EffortJointInterface\n\ + \ hardware_interface/PositionJointInterface\n\ + \ 1\n \n \n\ + \ \n \n \n \n \n\ + \ \n \n \n \n \ + \ \n \n \n \n \n\ + \ \n \n \n \n \n \n \n \n \n \n \n \n\ + \ \n \n \n \n \n \n 10000000.0\n\ + \ 1.0\n 0.005\n 100.0\n\ + \ Gazebo/DarkGrey\n \n \n transmission_interface/SimpleTransmission\n\ + \ \n hardware_interface/VelocityJointInterface\n\ + \ hardware_interface/EffortJointInterface\n\ + \ hardware_interface/PositionJointInterface\n\ + \ \n \n hardware_interface/VelocityJointInterface\n\ + \ hardware_interface/EffortJointInterface\n\ + \ hardware_interface/PositionJointInterface\n\ + \ 1\n \n \n\ + \ \n \n \n \n \n\ + \ \n \n \n \n \ + \ \n \n \n \n \n\ + \ \n \n \n \n \n \n \n \n \n \n \n \n\ + \ \n \n \n \n \n \n 10000000.0\n\ + \ 1.0\n 0.005\n 100.0\n\ + \ Gazebo/DarkGrey\n \n \n transmission_interface/SimpleTransmission\n\ + \ \n hardware_interface/VelocityJointInterface\n\ + \ hardware_interface/EffortJointInterface\n\ + \ hardware_interface/PositionJointInterface\n\ + \ \n \n hardware_interface/VelocityJointInterface\n\ + \ hardware_interface/EffortJointInterface\n\ + \ hardware_interface/PositionJointInterface\n\ + \ 1\n \n \n\ + \ \n \n \n \n \n \n \n \n\ + \ \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\ + \ \n \n \n \n \n \n 10000000.0\n\ + \ 1.0\n 0.005\n 100.0\n\ + \ Gazebo/DarkGrey\n \n \n transmission_interface/SimpleTransmission\n\ + \ \n hardware_interface/VelocityJointInterface\n\ + \ hardware_interface/EffortJointInterface\n\ + \ hardware_interface/PositionJointInterface\n\ + \ \n \n hardware_interface/VelocityJointInterface\n\ + \ hardware_interface/EffortJointInterface\n\ + \ hardware_interface/PositionJointInterface\n\ + \ 1\n \n \n\ + \ \n \n \n \n \n \n \n\ + \ \n \n \n \ + \ \n \n \n \ + \ \n \n \n \ + \ \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\ + \ \n \n \n \n \n \n \n \n \n \n \n \n Gazebo/Orange\n \n \n true\n \n\ + \ \n \n \n \n \n \n \n \n \n\ + \ \n \n \n \n \n \ + \ \n \n \n \n \n \n \n \n Gazebo/Red\n \n \n \n \n \n \n \n \n true\n\ + \ \n \n \n \n \n \n \n\ + \ \n \n \n \n \n \n \n \n \n \n\ + \ \n \n \n Gazebo/Grey\n\ + \ \n \n \n \n \n \n \n \n\ + \ \n \n \n \n\ + \ \n \n \n \ + \ \n \n \n \n \n \ + \ \n \n \n \n \n\ + \ \n \n \n \n \n \n \n \n \n\ + \ \n \n \n \n \n \n \ + \ \n \n \n \n \n \ + \ \n \n \n Gazebo/Black\n\ + \ \n \n \n \n \n \n \n \n true\n\ + \ \n \n \n \n \n \n \n\ + \ \n \n\n\n" +tracking_camera: + realsense2_camera: + accel_fps: 0 + accel_frame_id: tracking_camera_accel_frame + accel_optical_frame_id: tracking_camera_accel_optical_frame + align_depth: false + aligned_depth_to_color_frame_id: tracking_camera_aligned_depth_to_color_frame + aligned_depth_to_fisheye1_frame_id: tracking_camera_aligned_depth_to_fisheye1_frame + aligned_depth_to_fisheye2_frame_id: tracking_camera_aligned_depth_to_fisheye2_frame + aligned_depth_to_fisheye_frame_id: tracking_camera_aligned_depth_to_fisheye_frame + aligned_depth_to_infra1_frame_id: tracking_camera_aligned_depth_to_infra1_frame + aligned_depth_to_infra2_frame_id: tracking_camera_aligned_depth_to_infra2_frame + allow_no_texture_points: false + base_frame_id: tracking_camera_link + calib_odom_file: /home/robotx/catkin_workspaces/smb_dev/src/core/smb/config/tracking_camera_config.json + clip_distance: -1.0 + color_fps: 30 + color_frame_id: tracking_camera_color_frame + color_height: 480 + color_optical_frame_id: tracking_camera_color_optical_frame + color_width: 640 + confidence_fps: 30 + confidence_height: 480 + confidence_width: 640 + depth_fps: 30 + depth_frame_id: tracking_camera_depth_frame + depth_height: 480 + depth_optical_frame_id: tracking_camera_depth_optical_frame + depth_width: 640 + device_type: t265 + enable_accel: true + enable_color: true + enable_confidence: true + enable_depth: true + enable_fisheye: false + enable_fisheye1: false + enable_fisheye2: false + enable_gyro: true + enable_infra: false + enable_infra1: false + enable_infra2: false + enable_pointcloud: false + enable_pose: true + enable_sync: false + filters: '' + fisheye1_frame_id: tracking_camera_fisheye1_frame + fisheye1_optical_frame_id: tracking_camera_fisheye1_optical_frame + fisheye2_frame_id: tracking_camera_fisheye2_frame + fisheye2_optical_frame_id: tracking_camera_fisheye2_optical_frame + fisheye_fps: 30 + fisheye_frame_id: tracking_camera_fisheye_frame + fisheye_height: 0 + fisheye_optical_frame_id: tracking_camera_fisheye_optical_frame + fisheye_width: 0 + gyro_fps: 0 + gyro_frame_id: tracking_camera_gyro_frame + gyro_optical_frame_id: tracking_camera_gyro_optical_frame + imu_optical_frame_id: tracking_camera_imu_optical_frame + infra1_frame_id: tracking_camera_infra1_frame + infra1_optical_frame_id: tracking_camera_infra1_optical_frame + infra2_frame_id: tracking_camera_infra2_frame + infra2_optical_frame_id: tracking_camera_infra2_optical_frame + infra_fps: 30 + infra_height: 480 + infra_rgb: false + infra_width: 640 + initial_reset: false + json_file_path: '' + linear_accel_cov: 0.01 + odom_frame_id: tracking_camera_odom_frame + ordered_pc: false + pointcloud_texture_index: 0 + pointcloud_texture_stream: RS2_STREAM_COLOR + pose_frame_id: tracking_camera_pose_frame + pose_optical_frame_id: tracking_camera_pose_optical_frame + publish_odom_tf: false + publish_tf: false + reconnect_timeout: 6.0 + rosbag_filename: '' + serial_no: '' + stereo_module: + exposure: + '1': 7500 + '2': 1 + gain: + '1': 16 + '2': 16 + tf_publish_rate: 0.0 + topic_odom_in: /control/smb_diff_drive/odom + unite_imu_method: linear_interpolation + usb_port_id: '' + wait_for_device_timeout: -1.0 + tracking_module: + enable_mapping: false + enable_pose_jumping: false + enable_relocalization: false +versavis_imu_receiver: + imu_acceleration_covariance: 0.043864908 + imu_accelerator_sensitivity: 0.000833 + imu_gyro_covariance: 6e-9 + imu_gyro_sensitivity: 0.04 + imu_pub_topic: /imu + imu_sub_topic: /versavis/imu_micro diff --git a/examples/ros2/smb_estimator_graph_ros2/config/smb_specific/smb_extrinsic_params.yaml b/examples/ros2/smb_estimator_graph_ros2/config/smb_specific/smb_extrinsic_params.yaml new file mode 100644 index 00000000..9175f676 --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/config/smb_specific/smb_extrinsic_params.yaml @@ -0,0 +1,7 @@ +#External Prior/Transform Input +extrinsics: + lidarOdometryFrame: "rslidar" #LiDAR frame name - used to lookup LiDAR-to-ExternalSensor Frame + wheelOdometryBetweenFrame: "base_link" #Wheel odometry frame name - used to lookup Wheel-to-ExternalSensor Frame + wheelLinearVelocityLeftFrame: "LH_WHEEL" #Wheel linear velocity left frame name - used to lookup Wheel-to-ExternalSensor Frame + wheelLinearVelocityRightFrame: "RH_WHEEL" #Wheel linear velocity right frame name - used to lookup Wheel-to-ExternalSensor Frame + vioOdometryFrame: "base_link" #VIO odometry frame name - used to lookup VIO-to-ExternalSensor Frame diff --git a/examples/ros2/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml b/examples/ros2/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml new file mode 100644 index 00000000..7ef5c4df --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml @@ -0,0 +1,30 @@ +# Sensor Params +sensor_params: + ## Config + useLioOdometry: true + useWheelOdometryBetween: false + useWheelLinearVelocities: false + useVioOdometry: false + ## Rates + lioOdometryRate: 10 + wheelOdometryBetweenRate: 50 + wheelLinearVelocitiesRate: 50 + vioOdometryRate: 50 + # Wheel Radius + wheelRadius: 0.195 # meters + +# Alignment Parameters +alignment_params: + initialSe3AlignmentNoiseDensity: [ 1.0e-01, 1.0e-01, 1.0e-01, 1.0e-02, 1.0e-02, 1.0e-02 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + lioSe3AlignmentRandomWalk: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + +# Noise Parameters +noise_params: + ## LiDAR + lioPoseUnaryNoiseDensity: [ 0.05, 0.05, 0.05, 0.01, 0.01, 0.01 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + ## Wheel + wheelPoseBetweenNoiseDensity: [ 0.1, 0.1, 0.1, 0.06, 0.06, 0.06 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + wheelLinearVelocitiesNoiseDensity: [ 0.3, 0.3, 0.3 ] # StdDev, ORDER VxVyVz(meters/sec) + ## VIO + vioPoseBetweenNoiseDensity: [ 100.0, 100.0, 100.0, 0.1, 0.1, 0.1 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + vioPoseUnaryNoiseDensity: [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 ] # StdDev, ORDER RPY(rad) - XYZ(meters) diff --git a/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbEstimator.h b/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbEstimator.h new file mode 100644 index 00000000..be0cc9df --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbEstimator.h @@ -0,0 +1,124 @@ +/* +Copyright 2024 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +#ifndef Smb_Estimator_H +#define Smb_Estimator_H + +// std +#include + +// ROS +#include +#include +#include +#include +#include + +// Workspace +#include "graph_msf_ros/GraphMsfRos.h" + +// Defined Macros +#define ROS_QUEUE_SIZE 100 +#define NUM_GNSS_CALLBACKS_UNTIL_START 20 // 0 + +namespace smb_se { + +class SmbEstimator : public graph_msf::GraphMsfRos { + public: + SmbEstimator(std::shared_ptr privateNodePtr); + // Destructor + ~SmbEstimator() = default; + // Setup + void setup(); + + protected: + // Virtual Functions + void readParams(const ros::NodeHandle& privateNode) override; + void initializePublishers(ros::NodeHandle& privateNode) override; + void initializeSubscribers(ros::NodeHandle& privateNode) override; + void initializeMessages(ros::NodeHandle& privateNode) override; + void initializeServices(ros::NodeHandle& privateNode) override; + void imuCallback(const sensor_msgs::Imu::ConstPtr& imuPtr) override; + + private: + // Time + std::chrono::time_point startTime_; + std::chrono::time_point currentTime_; + + // Config ------------------------------------- + + // Rates + double lioOdometryRate_ = 5.0; + double wheelOdometryBetweenRate_ = 50.0; + double wheelLinearVelocitiesRate_ = 50.0; + double vioOdometryRate_ = 50.0; + + // Alignment Parameters + Eigen::Matrix initialSe3AlignmentNoise_ = 1.0 * Eigen::Matrix::Ones(); + Eigen::Matrix lioSe3AlignmentRandomWalk_ = 0.0 * Eigen::Matrix::Ones(); + + // Noise + // LiDAR Odometry + Eigen::Matrix lioPoseUnaryNoise_; + // Wheel Odometry + // Between + Eigen::Matrix wheelPoseBetweenNoise_; + // Linear Velocities + Eigen::Matrix wheelLinearVelocitiesNoise_; + // VIO Odometry + Eigen::Matrix vioPoseBetweenNoise_; + + // ROS Related stuff ---------------------------- + + // Callbacks + // LiDAR + void lidarOdometryCallback_(const nav_msgs::Odometry::ConstPtr& lidarOdomPtr); + // Wheel Between + void wheelOdometryPoseCallback_(const nav_msgs::Odometry::ConstPtr& wheelOdomPtr); + // Wheel Linear Velocities + void wheelLinearVelocitiesCallback_(const std_msgs::Float64MultiArray::ConstPtr& wheelsSpeedsPtr); + // VIO + void vioOdometryCallback_(const nav_msgs::Odometry::ConstPtr& vioOdomPtr); + + // Callback Members + int wheelOdometryCallbackCounter_ = -1; + Eigen::Isometry3d T_O_Bw_km1_; + double wheelOdometryTimeKm1_ = 0.0; + + // Subscribers + // Instances + // LIO + ros::Subscriber subLioOdometry_; + // Wheel Between + ros::Subscriber subWheelOdometryBetween_; + // Wheel Linear Velocities + ros::Subscriber subWheelLinearVelocities_; + // VIO + ros::Subscriber subVioOdometry_; + // TF Listener + tf::TransformListener tfListener_; + + // Publishers + // Path + ros::Publisher pubMeasMapLioPath_; + ros::Publisher pubMeasMapVioPath_; + + // Messages + nav_msgs::PathPtr measLio_mapImuPathPtr_; + nav_msgs::PathPtr measVio_mapImuPathPtr_; + + // Flags + bool useLioOdometryFlag_ = true; + bool useWheelOdometryBetweenFlag_ = false; + bool useWheelLinearVelocitiesFlag_ = false; + bool useVioOdometryFlag_ = false; + + // Wheel Radius + double wheelRadiusMeter_ = 0.195; +}; +} // namespace smb_se +#endif // end Smb_Estimator_H diff --git a/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h b/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h new file mode 100644 index 00000000..6312aa56 --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h @@ -0,0 +1,49 @@ +/* +Copyright 2023 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +#ifndef Smb_Static_Transforms_H +#define Smb_Static_Transforms_H +// Workspace +#include "graph_msf_ros/extrinsics/StaticTransformsTf.h" + +namespace smb_se { + +class SmbStaticTransforms : public graph_msf::StaticTransformsTf { + public: + SmbStaticTransforms(const std::shared_ptr privateNodePtr); + + // Setters + void setLioOdometryFrame(const std::string& s) { lidarOdometryFrame_ = s; } + void setWheelOdometryBetweenFrame(const std::string& s) { wheelOdometryBetweenFrame_ = s; } + void setWheelLinearVelocityLeftFrame(const std::string& s) { wheelLinearVelocityLeftFrame_ = s; } + void setWheelLinearVelocityRightFrame(const std::string& s) { wheelLinearVelocityRightFrame_ = s; } + void setVioOdometryFrame(const std::string& s) { vioOdometryFrame_ = s; } + + // Getters + const std::string& getLioOdometryFrame() { return lidarOdometryFrame_; } + const std::string& getWheelOdometryBetweenFrame() { return wheelOdometryBetweenFrame_; } + const std::string& getWheelLinearVelocityLeftFrame() { return wheelLinearVelocityLeftFrame_; } + const std::string& getWheelLinearVelocityRightFrame() { return wheelLinearVelocityRightFrame_; } + const std::string& getVioOdometryFrame() { return vioOdometryFrame_; } + + private: + void findTransformations() override; + + // Frames + // LiDAR + std::string lidarOdometryFrame_; + // Wheel + // Between + std::string wheelOdometryBetweenFrame_; + // Linear Velocities + std::string wheelLinearVelocityLeftFrame_; + std::string wheelLinearVelocityRightFrame_; + // VIO + std::string vioOdometryFrame_; +}; +} // namespace smb_se +#endif // end Smb_Static_Transforms_H diff --git a/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/constants.h b/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/constants.h new file mode 100644 index 00000000..f61e1f83 --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/constants.h @@ -0,0 +1,15 @@ +/* +Copyright 2024 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +#include "graph_msf/interface/Terminal.h" + +#ifndef SMB_CONSTANTS_H +#define SMB_CONSTANTS_H + +#define REGULAR_COUT std::cout << YELLOW_START << "SmbEstimator" << COLOR_END + +#endif // SMB_CONSTANTS_H diff --git a/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch b/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch new file mode 100644 index 00000000..01055b43 --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch b/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch new file mode 100644 index 00000000..049494fb --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/ros2/smb_estimator_graph_ros2/package.xml b/examples/ros2/smb_estimator_graph_ros2/package.xml new file mode 100644 index 00000000..949603dd --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/package.xml @@ -0,0 +1,32 @@ + + + smb_estimator_graph_ros2 + 0.0.0 + + + State estimation based on factor graphs, utilizing GTSAM functionality. Fusion of IMU, LiDAR Mapping Odometry + and GNSS estimates. + + + Julian Nubert + BSD-3-Clause + Julian Nubert + + + ament_cmake + + + graph_msf_ros2 + nav_msgs + std_msgs + sensor_msgs + tf2 + tf2_ros + rclcpp + graph_msf + glog + + + ament_cmake_gtest + rosbag2 + diff --git a/examples/ros2/smb_estimator_graph_ros2/rviz/lidar_estimation.rviz b/examples/ros2/smb_estimator_graph_ros2/rviz/lidar_estimation.rviz new file mode 100644 index 00000000..74de00d5 --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/rviz/lidar_estimation.rviz @@ -0,0 +1,729 @@ +Panels: + - Class: rviz/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /OdomImuOdometry1 + - /OdomImuOdometry1/Covariance1 + - /OdomImuOdometry1/Covariance1/Position1 + - /WorldImuOdometry1/Shape1 + - /WheelOdometry1/Shape1 + Splitter Ratio: 0.5529412031173706 + Tree Height: 1442 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: O3dMap +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: true + LF_WHEEL: + Value: true + LH_WHEEL: + Value: true + RF_WHEEL: + Value: true + RH_WHEEL: + Value: true + base_inertia: + Value: true + base_link: + Value: true + imu: + Value: true + imu_link: + Value: true + lidar_mount_link: + Value: true + map_o3d: + Value: true + map_o3d_viz: + Value: true + odom: + Value: true + odom_graph_msf: + Value: true + raw_rs_o3d: + Value: true + rgb_camera_link: + Value: true + rgb_camera_optical_link: + Value: true + rslidar: + Value: true + top: + Value: true + tracking_camera_link: + Value: true + tracking_camera_pose_frame: + Value: true + world_graph_msf: + Value: true + Marker Alpha: 1 + Marker Scale: 0.10000000149011612 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: true + Tree: + base_link: + LF_WHEEL: + {} + LH_WHEEL: + {} + RF_WHEEL: + {} + RH_WHEEL: + {} + base_inertia: + {} + imu_link: + imu: + {} + lidar_mount_link: + rslidar: + map_o3d: + map_o3d_viz: + {} + raw_rs_o3d: + {} + odom_graph_msf: + world_graph_msf: + odom: + {} + rgb_camera_link: + rgb_camera_optical_link: + {} + top: + {} + tracking_camera_link: + tracking_camera_pose_frame: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: z + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 16.27729034423828 + Min Color: 0; 0; 0 + Min Intensity: -3.2934958934783936 + Name: FilteredPointCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /rslidar/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation + Color Transformer: GridMapLayer + Enabled: true + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: true + Topic: /elevation_mapping/elevation_map + Unreliable: false + Use Rainbow: true + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 255; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: OdomImuEstimated + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /graph_msf/est_path_odom_imu + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 114; 159; 207 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: WorldImuEstimated + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /graph_msf/est_path_world_imu + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: lio_MapImuMeasured + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /graph_msf/measLiDAR_path_map_imu + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 193; 125; 17 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: vio_MapImuMeasured + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /graph_msf/measVIO_path_map_imu + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 191; 64; 170 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: MapImuOptimized + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /graph_msf/opt_path_world_imu + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: CompslamMap + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 4 + Size (m): 0.009999999776482582 + Style: Points + Topic: /compslam_lio/laser_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: CompslamRegisteredCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /compslam_lio/registered_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: z + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: O3dMap + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /mapping/assembled_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: O3dRegisteredCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /mapping_node/raw_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Angle Tolerance: 0 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: false + Position: + Alpha: 0.30000001192092896 + Color: 239; 41; 41 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: OdomImuOdometry + Position Tolerance: 0 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 255; 255 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /graph_msf/est_odometry_odom_imu + Unreliable: false + Value: true + - Angle Tolerance: 0 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 1 + Name: WorldImuOdometry + Position Tolerance: 0 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 114; 159; 207 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /graph_msf/est_odometry_world_imu + Unreliable: false + Value: true + - Angle Tolerance: 0 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 1 + Name: O3dOdometry + Position Tolerance: 0 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 138; 226; 52 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /mapping_node/scan2map_odometry + Unreliable: false + Value: false + - Angle Tolerance: 0 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: WheelOdometry + Position Tolerance: 0 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /control/smb_diff_drive/odom + Unreliable: false + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + LF_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LH_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + RF_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lidar_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rgb_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rgb_camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + rslidar: + Alpha: 1 + Show Axes: false + Show Trail: false + rslidar_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tracking_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tracking_camera_pose_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: smb_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Default Light: true + Fixed Frame: odom_graph_msf + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7853981852531433 + Target Frame: + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1846 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000002290000069afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730200000c00000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e0000069a0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f0000069afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000006e0000069a0000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000064f0000003efc0100000002fb0000000800540069006d006500000000000000064f0000077d00fffffffb0000000800540069006d00650100000000000004500000000000000000000009cb0000069a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 3072 + X: 0 + Y: 960 diff --git a/examples/ros2/smb_estimator_graph_ros2/scripts/remove_map_and_odom_from_bag.py b/examples/ros2/smb_estimator_graph_ros2/scripts/remove_map_and_odom_from_bag.py new file mode 100644 index 00000000..92495df9 --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/scripts/remove_map_and_odom_from_bag.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python3 + +# Code taken from: https://gist.github.com/awesomebytes/51470efe54b45045c50263f56d7aec27 + +import rosbag +import sys + +def filter_topics(in_bag, out_bag, frames_we_want): + with rosbag.Bag(out_bag, 'w') as outbag: + print("Writing to " + out_bag) + print("Reading from " + in_bag) + for topic, msg, t in rosbag.Bag(in_bag).read_messages(): + if topic == "/tf" and msg.transforms: + transforms_to_keep = [] + for i in range(len(msg.transforms)): + # if its one of the frames we want we keep it + if msg.transforms[i].header.frame_id in frames_we_want and msg.transforms[i].child_frame_id in frames_we_want: + transforms_to_keep.append(msg.transforms[i]) + #print("Keeping: " + str(msg.transforms[i])) + # else: + # print("Discarding: " + str(msg.transforms[i])) + + msg.transforms = transforms_to_keep + outbag.write(topic, msg, t) + elif topic != '/tf': + outbag.write(topic, msg, t) + +def filter_topics_2(in_bag, out_bag, frames_we_dont_want): + with rosbag.Bag(out_bag, 'w') as outbag: + print("Writing to " + out_bag) + print("Reading from " + in_bag) + for topic, msg, t in rosbag.Bag(in_bag).read_messages(): + if topic == "/tf" and msg.transforms: + transforms_to_keep = [] + for i in range(len(msg.transforms)): + # if its one of the frames we want we keep it + if msg.transforms[i].header.frame_id not in frames_we_dont_want and msg.transforms[i].child_frame_id not in frames_we_dont_want: + transforms_to_keep.append(msg.transforms[i]) + #print("Keeping: " + str(msg.transforms[i])) + # else: + # print("Discarding: " + str(msg.transforms[i])) + + msg.transforms = transforms_to_keep + outbag.write(topic, msg, t) + elif topic != '/tf': + outbag.write(topic, msg, t) + + +if __name__ == '__main__': + print("Starting") + in_bag = sys.argv[1] + out_bag = sys.argv[2] + # filter_topics(in_bag, out_bag, ['base_link', 'odom', 'map', + # 'torso', 'Hip', 'Pelvis', 'Tibia', 'base_footprint']) + filter_topics_2(in_bag, out_bag, ['odom']) + print("Done") \ No newline at end of file diff --git a/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp b/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp new file mode 100644 index 00000000..75518012 --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp @@ -0,0 +1,312 @@ +/* +Copyright 2024 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +// Implementation +#include "smb_estimator_graph/SmbEstimator.h" + +// Project +#include "smb_estimator_graph/SmbStaticTransforms.h" + +// Workspace +#include "graph_msf/measurements/BinaryMeasurementXD.h" +#include "graph_msf/measurements/UnaryMeasurementXD.h" +#include "graph_msf_ros/util/conversions.h" +#include "smb_estimator_graph/constants.h" + +namespace smb_se { + +SmbEstimator::SmbEstimator(std::shared_ptr privateNodePtr) : graph_msf::GraphMsfRos(privateNodePtr) { + REGULAR_COUT << GREEN_START << " SmbEstimator-Constructor called." << COLOR_END << std::endl; + + // Configurations ---------------------------- + // Static transforms + staticTransformsPtr_ = std::make_shared(privateNodePtr); + + // Set up + SmbEstimator::setup(); +} + +void SmbEstimator::setup() { + REGULAR_COUT << GREEN_START << " SmbEstimator-Setup called." << COLOR_END << std::endl; + + // Read parameters ---------------------------- + SmbEstimator::readParams(privateNode); + + // Super class + GraphMsfRos::setup(staticTransformsPtr_); + + // Publishers ---------------------------- + SmbEstimator::initializePublishers(privateNode); + + // Subscribers ---------------------------- + SmbEstimator::initializeSubscribers(privateNode); + + // Messages ---------------------------- + SmbEstimator::initializeMessages(privateNode); + + // Services ---------------------------- + SmbEstimator::initializeServices(privateNode); + + // Transforms ---------------------------- + staticTransformsPtr_->findTransformations(); + + // Wrap up ---------------------------- + REGULAR_COUT << GREEN_START << " Set up successfully." << COLOR_END << std::endl; +} + +void SmbEstimator::initializePublishers(ros::NodeHandle& privateNode) { + // Paths + pubMeasMapLioPath_ = privateNode.advertise("/graph_msf/measLiDAR_path_map_imu", ROS_QUEUE_SIZE); + pubMeasMapVioPath_ = privateNode.advertise("/graph_msf/measVIO_path_map_imu", ROS_QUEUE_SIZE); +} + +void SmbEstimator::initializeSubscribers(ros::NodeHandle& privateNode) { + // LiDAR Odometry + if (useLioOdometryFlag_) { + subLioOdometry_ = privateNode.subscribe( + "/lidar_odometry_topic", ROS_QUEUE_SIZE, &SmbEstimator::lidarOdometryCallback_, this, ros::TransportHints().tcpNoDelay()); + REGULAR_COUT << COLOR_END << " Initialized LiDAR Odometry subscriber with topic: " << subLioOdometry_.getTopic() << std::endl; + } + + // Wheel Odometry Between + if (useWheelOdometryBetweenFlag_) { + subWheelOdometryBetween_ = privateNode.subscribe( + "/wheel_odometry_topic", ROS_QUEUE_SIZE, &SmbEstimator::wheelOdometryPoseCallback_, this, ros::TransportHints().tcpNoDelay()); + REGULAR_COUT << COLOR_END << " Initialized Wheel Odometry subscriber with topic: " << subWheelOdometryBetween_.getTopic() << std::endl; + } + + // Wheel Linear Velocities + if (useWheelLinearVelocitiesFlag_) { + subWheelLinearVelocities_ = privateNode.subscribe( + "/wheel_velocities_topic", ROS_QUEUE_SIZE, &SmbEstimator::wheelLinearVelocitiesCallback_, this, ros::TransportHints().tcpNoDelay()); + REGULAR_COUT << COLOR_END << " Initialized Wheel Linear Velocities subscriber with topic: " << subWheelLinearVelocities_.getTopic() + << std::endl; + } + + // VIO + if (useVioOdometryFlag_) { + subVioOdometry_ = privateNode.subscribe("/vio_odometry_topic", ROS_QUEUE_SIZE, &SmbEstimator::vioOdometryCallback_, + this, ros::TransportHints().tcpNoDelay()); + REGULAR_COUT << COLOR_END << " Initialized VIO Odometry subscriber with topic: " << subVioOdometry_.getTopic() << std::endl; + } +} + +void SmbEstimator::initializeMessages(ros::NodeHandle& privateNode) { + // Path + measLio_mapImuPathPtr_ = nav_msgs::PathPtr(new nav_msgs::Path); + measVio_mapImuPathPtr_ = nav_msgs::PathPtr(new nav_msgs::Path); +} + +void SmbEstimator::initializeServices(ros::NodeHandle& privateNode) { + // Nothing for now +} + +void SmbEstimator::imuCallback(const sensor_msgs::Imu::ConstPtr& imuPtr) { + // Check whether any of the measurements is available, otherwise do pure imu integration + if (graph_msf::GraphMsf::areRollAndPitchInited() && !graph_msf::GraphMsf::areYawAndPositionInited() && !useLioOdometryFlag_ && + !useWheelOdometryBetweenFlag_ && !useWheelLinearVelocitiesFlag_ && !useVioOdometryFlag_) { + // Initialization + REGULAR_COUT << RED_START << " IMU callback is setting global yaw and position, as no other odometry is available. Initializing..." + << COLOR_END << std::endl; + // Create dummy measurement for initialization + graph_msf::UnaryMeasurementXD unary6DMeasurement( + "IMU_init_6D", int(graphConfigPtr_->imuRate_), staticTransformsPtr_->getImuFrame(), + staticTransformsPtr_->getImuFrame() + sensorFrameCorrectedNameId, graph_msf::RobustNorm::None(), imuPtr->header.stamp.toSec(), 1.0, + Eigen::Isometry3d::Identity(), Eigen::MatrixXd::Identity(6, 1)); + // Initialize + graph_msf::GraphMsf::initYawAndPosition(unary6DMeasurement); + REGULAR_COUT << RED_START << " ...initialized yaw and position to identity." << COLOR_END << std::endl; + // Pretend that we received first measurement --> In order to allow for optimization + graph_msf::GraphMsf::pretendFirstMeasurementReceived(); + } + + // Super class + graph_msf::GraphMsfRos::imuCallback(imuPtr); +} + +void SmbEstimator::lidarOdometryCallback_(const nav_msgs::Odometry::ConstPtr& odomLidarPtr) { + // Static members + static int lidarOdometryCallbackCounter__ = -1; + + // Counter + ++lidarOdometryCallbackCounter__; + + Eigen::Isometry3d lio_T_M_Lk; + graph_msf::odomMsgToEigen(*odomLidarPtr, lio_T_M_Lk.matrix()); + + // Transform to IMU frame + double lidarOdometryTimeK = odomLidarPtr->header.stamp.toSec(); + + // Frame Name + const std::string& lioOdometryFrame = dynamic_cast(staticTransformsPtr_.get())->getLioOdometryFrame(); // alias + + // Measurement + graph_msf::UnaryMeasurementXDAbsolute unary6DMeasurement( + "Lidar_unary_6D", int(lioOdometryRate_), lioOdometryFrame, lioOdometryFrame + sensorFrameCorrectedNameId, + graph_msf::RobustNorm::None(), lidarOdometryTimeK, 1.0, lio_T_M_Lk, lioPoseUnaryNoise_, odomLidarPtr->header.frame_id, + staticTransformsPtr_->getWorldFrame(), initialSe3AlignmentNoise_, lioSe3AlignmentRandomWalk_); + + // Add measurement or initialize + if (lidarOdometryCallbackCounter__ <= 2) { + return; + } else if (areYawAndPositionInited()) { // Already initialized --> unary factor + this->addUnaryPose3AbsoluteMeasurement(unary6DMeasurement); + } else { // Initializing + REGULAR_COUT << GREEN_START << " LiDAR odometry callback is setting global yaw, as it was not set so far." << COLOR_END << std::endl; + this->initYawAndPosition(unary6DMeasurement); + } + + // Visualization ---------------------------- + // Add to path message + addToPathMsg(measLio_mapImuPathPtr_, odomLidarPtr->header.frame_id, odomLidarPtr->header.stamp, + (lio_T_M_Lk * staticTransformsPtr_ + ->rv_T_frame1_frame2(dynamic_cast(staticTransformsPtr_.get())->getLioOdometryFrame(), + staticTransformsPtr_->getImuFrame()) + .matrix()) + .block<3, 1>(0, 3), + graphConfigPtr_->imuBufferLength_ * 4); + + // Publish Path + pubMeasMapLioPath_.publish(measLio_mapImuPathPtr_); +} + +void SmbEstimator::wheelOdometryPoseCallback_(const nav_msgs::Odometry::ConstPtr& wheelOdometryKPtr) { + if (!areRollAndPitchInited()) { + return; + } + + // Counter + ++wheelOdometryCallbackCounter_; + + // Eigen Type + Eigen::Isometry3d T_O_Bw_k; + graph_msf::odomMsgToEigen(*wheelOdometryKPtr, T_O_Bw_k.matrix()); + double wheelOdometryTimeK = wheelOdometryKPtr->header.stamp.toSec(); + + // At start + if (wheelOdometryCallbackCounter_ == 0) { + T_O_Bw_km1_ = T_O_Bw_k; + wheelOdometryTimeKm1_ = wheelOdometryTimeK; + return; + } + + // Frame name + const std::string& wheelOdometryFrame = + dynamic_cast(staticTransformsPtr_.get())->getWheelOdometryBetweenFrame(); // alias + + // State Machine + if (!areYawAndPositionInited()) { + // If no LIO, then initialize the graph here + if (!useLioOdometryFlag_) { + REGULAR_COUT << GREEN_START << " Wheel odometry callback is setting global yaw and position, as lio is all set to false." << COLOR_END + << std::endl; + graph_msf::UnaryMeasurementXD unary6DMeasurement( + "Lidar_unary_6D", int(wheelOdometryBetweenRate_), wheelOdometryFrame, wheelOdometryFrame + sensorFrameCorrectedNameId, + graph_msf::RobustNorm::None(), wheelOdometryTimeK, 1.0, Eigen::Isometry3d::Identity(), Eigen::MatrixXd::Identity(6, 1)); + graph_msf::GraphMsf::initYawAndPosition(unary6DMeasurement); + REGULAR_COUT << " Initialized yaw and position to identity in the wheel odometry callback, as lio and vio are all set to false." + << std::endl; + } + } else { + // Only add every 5th measurement + int measurementRate = static_cast(wheelOdometryBetweenRate_ / 5); + // Check + if (wheelOdometryCallbackCounter_ % 5 == 0 && wheelOdometryCallbackCounter_ > 0) { + // Compute Delta + Eigen::Isometry3d T_Bkm1_Bk = T_O_Bw_km1_.inverse() * T_O_Bw_k; + // Create measurement + graph_msf::BinaryMeasurementXD delta6DMeasurement( + "Wheel_odometry_6D", measurementRate, wheelOdometryFrame, wheelOdometryFrame + sensorFrameCorrectedNameId, + graph_msf::RobustNorm::Tukey(1.0), wheelOdometryTimeKm1_, wheelOdometryTimeK, T_Bkm1_Bk, wheelPoseBetweenNoise_); + // Add to graph + this->addBinaryPose3Measurement(delta6DMeasurement); + + // Prepare for next iteration + T_O_Bw_km1_ = T_O_Bw_k; + wheelOdometryTimeKm1_ = wheelOdometryTimeK; + } + } +} + +void SmbEstimator::wheelLinearVelocitiesCallback_(const std_msgs::Float64MultiArray::ConstPtr& wheelsSpeedsPtr) { + if (!areRollAndPitchInited()) { + return; + } + + // Get the 3 values + const double timeK = wheelsSpeedsPtr->data[0]; + const double leftWheelSpeedRps = wheelsSpeedsPtr->data[1]; + const double rightWheelSpeedRps = wheelsSpeedsPtr->data[2]; + const double leftWheelSpeedMs = leftWheelSpeedRps * wheelRadiusMeter_; + const double rightWheelSpeedMs = rightWheelSpeedRps * wheelRadiusMeter_; + + // Print + if (graphConfigPtr_->verboseLevel_ > 1) { + REGULAR_COUT << " Wheel linear velocities callback: " << std::setprecision(14) << timeK << " " << leftWheelSpeedMs << " [m/s] " + << rightWheelSpeedMs << " [m/s]" << std::endl; + } + + // Frame name + const std::string& wheelLinearVelocityLeftFrame = + dynamic_cast(staticTransformsPtr_.get())->getWheelLinearVelocityLeftFrame(); // alias + const std::string& wheelLinearVelocityRightFrame = + dynamic_cast(staticTransformsPtr_.get())->getWheelLinearVelocityRightFrame(); // alias + + // State Machine + if (!areYawAndPositionInited()) { + // If no LIO, then initialize the graph here + if (!useLioOdometryFlag_ && !useWheelOdometryBetweenFlag_) { + REGULAR_COUT << GREEN_START << " Wheel linear velocities callback is setting global yaw and position, as lio is all set to false." + << COLOR_END << std::endl; + graph_msf::UnaryMeasurementXD unary6DMeasurement( + "Lidar_unary_6D", int(wheelLinearVelocitiesRate_), wheelLinearVelocityLeftFrame, + wheelLinearVelocityLeftFrame + sensorFrameCorrectedNameId, graph_msf::RobustNorm::None(), timeK, 1.0, + Eigen::Isometry3d::Identity(), Eigen::MatrixXd::Identity(6, 1)); + graph_msf::GraphMsf::initYawAndPosition(unary6DMeasurement); + REGULAR_COUT + << " Initialized yaw and position to identity in the wheel linear velocities callback, as lio and vio are all set to false." + << std::endl; + } + } else { + // Left Wheel + graph_msf::UnaryMeasurementXD leftWheelLinearVelocityMeasurement( + "Wheel_linear_velocity_left", int(wheelLinearVelocitiesRate_), wheelLinearVelocityLeftFrame, + wheelLinearVelocityLeftFrame + sensorFrameCorrectedNameId, graph_msf::RobustNorm::Tukey(1.0), timeK, 1.0, + Eigen::Vector3d(leftWheelSpeedMs, 0.0, 0.0), wheelLinearVelocitiesNoise_); + this->addUnaryVelocity3LocalMeasurement(leftWheelLinearVelocityMeasurement); + + // Right Wheel + graph_msf::UnaryMeasurementXD rightWheelLinearVelocityMeasurement( + "Wheel_linear_velocity_right", int(wheelLinearVelocitiesRate_), wheelLinearVelocityRightFrame, + wheelLinearVelocityRightFrame + sensorFrameCorrectedNameId, graph_msf::RobustNorm::Tukey(1.0), timeK, 1.0, + Eigen::Vector3d(rightWheelSpeedMs, 0.0, 0.0), wheelLinearVelocitiesNoise_); + this->addUnaryVelocity3LocalMeasurement(rightWheelLinearVelocityMeasurement); + } +} + +void SmbEstimator::vioOdometryCallback_(const nav_msgs::Odometry::ConstPtr& vioOdomPtr) { + std::cout << "VIO odometry not yet stable enough for usage, disable flag." << std::endl; + + // Extract + Eigen::Isometry3d vio_T_M_Ck; + graph_msf::odomMsgToEigen(*vioOdomPtr, vio_T_M_Ck.matrix()); + + // Visualization ---------------------------- + // Add to path message + addToPathMsg(measVio_mapImuPathPtr_, vioOdomPtr->header.frame_id, vioOdomPtr->header.stamp, + (vio_T_M_Ck * staticTransformsPtr_ + ->rv_T_frame1_frame2(dynamic_cast(staticTransformsPtr_.get())->getVioOdometryFrame(), + staticTransformsPtr_->getImuFrame()) + .matrix()) + .block<3, 1>(0, 3), + graphConfigPtr_->imuBufferLength_ * 4 * 10); + + // Publish Path + pubMeasMapVioPath_.publish(measVio_mapImuPathPtr_); +} + +} // namespace smb_se \ No newline at end of file diff --git a/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp b/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp new file mode 100644 index 00000000..ef7467cb --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp @@ -0,0 +1,102 @@ +/* +Copyright 2023 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +// Implementation +#include "smb_estimator_graph/SmbStaticTransforms.h" + +// ROS +#include +#include + +// Workspace +#include "graph_msf_ros/util/conversions.h" +#include "smb_estimator_graph/constants.h" + +namespace smb_se { + +SmbStaticTransforms::SmbStaticTransforms(const std::shared_ptr privateNodePtr) : graph_msf::StaticTransformsTf() { + REGULAR_COUT << GREEN_START << " Initializing static transforms..." << COLOR_END << std::endl; +} + +void SmbStaticTransforms::findTransformations() { + // Super Method + graph_msf::StaticTransformsTf::findTransformations(); + + // Print to console -------------------------- + REGULAR_COUT << COLOR_END << " Looking up transforms in TF-tree." << std::endl; + REGULAR_COUT << COLOR_END << " Transforms between the following frames are required:" << std::endl; + REGULAR_COUT << COLOR_END << " Waiting for up to 100 seconds until they arrive..." << std::endl; + + // Temporary variable + static tf::StampedTransform transform; + + // Look up transforms ---------------------------- + ros::Rate rosRate(10); + rosRate.sleep(); + + // Imu to LiDAR Link --- + REGULAR_COUT << COLOR_END << " Waiting for transform for 10 seconds."; + listener_.waitForTransform(imuFrame_, lidarOdometryFrame_, ros::Time(0), ros::Duration(1.0)); + listener_.lookupTransform(imuFrame_, lidarOdometryFrame_, ros::Time(0), transform); + // I_Lidar + graph_msf::tfToIsometry3(tf::Transform(transform), lv_T_frame1_frame2(imuFrame_, lidarOdometryFrame_)); + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END + << " Translation I_Lidar: " << rv_T_frame1_frame2(imuFrame_, lidarOdometryFrame_).translation() << std::endl; + // Lidar_I + lv_T_frame1_frame2(lidarOdometryFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, lidarOdometryFrame_).inverse(); + + // Imu to VIO Link --- + REGULAR_COUT << COLOR_END << " Waiting for transform for 10 seconds."; + listener_.waitForTransform(imuFrame_, vioOdometryFrame_, ros::Time(0), ros::Duration(1.0)); + listener_.lookupTransform(imuFrame_, vioOdometryFrame_, ros::Time(0), transform); + // I_VIO + graph_msf::tfToIsometry3(tf::Transform(transform), lv_T_frame1_frame2(imuFrame_, vioOdometryFrame_)); + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END + << " Translation I_VIO: " << rv_T_frame1_frame2(imuFrame_, vioOdometryFrame_).translation() << std::endl; + // VIO_I + lv_T_frame1_frame2(vioOdometryFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, vioOdometryFrame_).inverse(); + + // Wheel Frames --- + REGULAR_COUT + << RED_START + << " As the Wheels are turning, we only use the position of the wheels frames and use the orientation of the baseLinkFrame_ frame." + << COLOR_END << std::endl; + // Left Wheel + REGULAR_COUT << COLOR_END << " Waiting for transform for 10 seconds."; + listener_.waitForTransform(imuFrame_, wheelLinearVelocityLeftFrame_, ros::Time(0), ros::Duration(1.0)); + listener_.lookupTransform(imuFrame_, wheelLinearVelocityLeftFrame_, ros::Time(0), transform); + // I_WheelLeft + Eigen::Isometry3d T_I_WheelLeft; + graph_msf::tfToIsometry3(tf::Transform(transform), T_I_WheelLeft); + // Overwrite the orientation with the one of the baseLinkFrame_ + T_I_WheelLeft.matrix().block<3, 3>(0, 0) = rv_T_frame1_frame2(imuFrame_, baseLinkFrame_).matrix().block<3, 3>(0, 0); + lv_T_frame1_frame2(imuFrame_, wheelLinearVelocityLeftFrame_) = T_I_WheelLeft; + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END + << " Translation I_WheelLeft: " << rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityLeftFrame_).translation() + << ", Rotation: " << T_I_WheelLeft.matrix().block<3, 3>(0, 0) << std::endl; + // WheelLeft_I + lv_T_frame1_frame2(wheelLinearVelocityLeftFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityLeftFrame_).inverse(); + // Right Wheel + REGULAR_COUT << COLOR_END << " Waiting for transform for 10 seconds."; + listener_.waitForTransform(imuFrame_, wheelLinearVelocityRightFrame_, ros::Time(0), ros::Duration(1.0)); + listener_.lookupTransform(imuFrame_, wheelLinearVelocityRightFrame_, ros::Time(0), transform); + // I_WheelRight + Eigen::Isometry3d T_I_WheelRight; + graph_msf::tfToIsometry3(tf::Transform(transform), T_I_WheelRight); + // Overwrite the orientation with the one of the baseLinkFrame_ + T_I_WheelRight.matrix().block<3, 3>(0, 0) = rv_T_frame1_frame2(imuFrame_, baseLinkFrame_).matrix().block<3, 3>(0, 0); + lv_T_frame1_frame2(imuFrame_, wheelLinearVelocityRightFrame_) = T_I_WheelRight; + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END + << " Translation I_WheelRight: " << rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityRightFrame_).translation() + << ", Rotation: " << T_I_WheelRight.matrix().block<3, 3>(0, 0) << std::endl; + // WheelRight_I + lv_T_frame1_frame2(wheelLinearVelocityRightFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityRightFrame_).inverse(); + + REGULAR_COUT << GREEN_START << " Transforms looked up successfully." << COLOR_END << std::endl; +} + +} // namespace smb_se diff --git a/examples/ros2/smb_estimator_graph_ros2/src/lib/readParams.cpp b/examples/ros2/smb_estimator_graph_ros2/src/lib/readParams.cpp new file mode 100644 index 00000000..a06340cf --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/src/lib/readParams.cpp @@ -0,0 +1,92 @@ +/* +Copyright 2024 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +// Implementation +#include "smb_estimator_graph/SmbEstimator.h" + +// Project +#include "smb_estimator_graph/SmbStaticTransforms.h" +#include "smb_estimator_graph/constants.h" + +// GraphMSF ROS +#include "graph_msf_ros/ros/read_ros_params.h" + +namespace smb_se { + +void SmbEstimator::readParams(const ros::NodeHandle& privateNode) { + // Check + if (!graphConfigPtr_) { + throw std::runtime_error("SmbEstimator: graphConfigPtr must be initialized."); + } + + // Flags + useLioOdometryFlag_ = graph_msf::tryGetParam("sensor_params/useLioOdometry", privateNode); + useWheelOdometryBetweenFlag_ = graph_msf::tryGetParam("sensor_params/useWheelOdometryBetween", privateNode); + useWheelLinearVelocitiesFlag_ = graph_msf::tryGetParam("sensor_params/useWheelLinearVelocities", privateNode); + useVioOdometryFlag_ = graph_msf::tryGetParam("sensor_params/useVioOdometry", privateNode); + + // Sensor Params + lioOdometryRate_ = graph_msf::tryGetParam("sensor_params/lioOdometryRate", privateNode); + wheelOdometryBetweenRate_ = graph_msf::tryGetParam("sensor_params/wheelOdometryBetweenRate", privateNode); + wheelLinearVelocitiesRate_ = graph_msf::tryGetParam("sensor_params/wheelLinearVelocitiesRate", privateNode); + vioOdometryRate_ = graph_msf::tryGetParam("sensor_params/vioOdometryRate", privateNode); + + // Alignment Parameters + const auto initialSe3AlignmentNoiseDensity = + graph_msf::tryGetParam>("alignment_params/initialSe3AlignmentNoiseDensity", privateNode); + initialSe3AlignmentNoise_ << initialSe3AlignmentNoiseDensity[0], initialSe3AlignmentNoiseDensity[1], initialSe3AlignmentNoiseDensity[2], + initialSe3AlignmentNoiseDensity[3], initialSe3AlignmentNoiseDensity[4], initialSe3AlignmentNoiseDensity[5]; + const auto lioSe3AlignmentRandomWalk = + graph_msf::tryGetParam>("alignment_params/lioSe3AlignmentRandomWalk", privateNode); + lioSe3AlignmentRandomWalk_ << lioSe3AlignmentRandomWalk[0], lioSe3AlignmentRandomWalk[1], lioSe3AlignmentRandomWalk[2], + lioSe3AlignmentRandomWalk[3], lioSe3AlignmentRandomWalk[4], lioSe3AlignmentRandomWalk[5]; + + // Noise Parameters + /// LiDAR Odometry + const auto poseUnaryNoise = + graph_msf::tryGetParam>("noise_params/lioPoseUnaryNoiseDensity", privateNode); // roll,pitch,yaw,x,y,z + lioPoseUnaryNoise_ << poseUnaryNoise[0], poseUnaryNoise[1], poseUnaryNoise[2], poseUnaryNoise[3], poseUnaryNoise[4], poseUnaryNoise[5]; + /// Wheel Odometry + /// Between + const auto wheelPoseBetweenNoise = + graph_msf::tryGetParam>("noise_params/wheelPoseBetweenNoiseDensity", privateNode); // roll,pitch,yaw,x,y,z + wheelPoseBetweenNoise_ << wheelPoseBetweenNoise[0], wheelPoseBetweenNoise[1], wheelPoseBetweenNoise[2], wheelPoseBetweenNoise[3], + wheelPoseBetweenNoise[4], wheelPoseBetweenNoise[5]; + /// Linear Velocities + const auto wheelLinearVelocitiesNoise = + graph_msf::tryGetParam>("noise_params/wheelLinearVelocitiesNoiseDensity", privateNode); // left,right + wheelLinearVelocitiesNoise_ << wheelLinearVelocitiesNoise[0], wheelLinearVelocitiesNoise[1], wheelLinearVelocitiesNoise[2]; + /// VIO Odometry + const auto vioPoseBetweenNoise = + graph_msf::tryGetParam>("noise_params/vioPoseBetweenNoiseDensity", privateNode); // roll,pitch,yaw,x,y,z + vioPoseBetweenNoise_ << vioPoseBetweenNoise[0], vioPoseBetweenNoise[1], vioPoseBetweenNoise[2], vioPoseBetweenNoise[3], + vioPoseBetweenNoise[4], vioPoseBetweenNoise[5]; + + // Set frames + /// LiDAR odometry frame + dynamic_cast(staticTransformsPtr_.get()) + ->setLioOdometryFrame(graph_msf::tryGetParam("extrinsics/lidarOdometryFrame", privateNode)); + /// Wheel Odometry frame + dynamic_cast(staticTransformsPtr_.get()) + ->setWheelOdometryBetweenFrame(graph_msf::tryGetParam("extrinsics/wheelOdometryBetweenFrame", privateNode)); + /// Whel Linear Velocities frames + /// Left + dynamic_cast(staticTransformsPtr_.get()) + ->setWheelLinearVelocityLeftFrame(graph_msf::tryGetParam("extrinsics/wheelLinearVelocityLeftFrame", privateNode)); + /// Right + dynamic_cast(staticTransformsPtr_.get()) + ->setWheelLinearVelocityRightFrame(graph_msf::tryGetParam("extrinsics/wheelLinearVelocityRightFrame", privateNode)); + + /// VIO Odometry frame + dynamic_cast(staticTransformsPtr_.get()) + ->setVioOdometryFrame(graph_msf::tryGetParam("extrinsics/vioOdometryFrame", privateNode)); + + // Wheel Radius + wheelRadiusMeter_ = graph_msf::tryGetParam("sensor_params/wheelRadius", privateNode); +} + +} // namespace smb_se diff --git a/examples/ros2/smb_estimator_graph_ros2/src/smb_estimator_node.cpp b/examples/ros2/smb_estimator_graph_ros2/src/smb_estimator_node.cpp new file mode 100644 index 00000000..4296efae --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/src/smb_estimator_node.cpp @@ -0,0 +1,36 @@ +/* +Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. +All rights reserved. +This file is released under the "BSD-3-Clause License". +Please see the LICENSE file that has been included as part of this package. + */ + +// ROS +#include + +// Debugging +#include +#include + +// Local packages +#include "smb_estimator_graph/SmbEstimator.h" + +// Main node entry point +int main(int argc, char** argv) { + // Debugging + google::InitGoogleLogging(argv[0]); + FLAGS_alsologtostderr = true; + google::InstallFailureSignalHandler(); + + // ROS related + ros::init(argc, argv, "anymal_estimator_graph"); + std::shared_ptr privateNodePtr = std::make_shared("~"); + /// Do multi-threaded spinner + ros::MultiThreadedSpinner spinner(4); + + // Create Instance + smb_se::SmbEstimator anymalEstimator(privateNodePtr); + spinner.spin(); + + return 0; +} \ No newline at end of file From 9aa32bce4a45cfe6a8a7a5331655ebe14029a2b9 Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Wed, 21 May 2025 15:34:43 +0000 Subject: [PATCH 06/60] Update CMakeLists.txt to reference the correct constants header for smb_estimator_graph_ros2 --- examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt b/examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt index 7fd88e52..7ce7fc38 100644 --- a/examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt +++ b/examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt @@ -47,7 +47,7 @@ add_library(${PROJECT_NAME} src/lib/SmbEstimator.cpp src/lib/readParams.cpp src/lib/SmbStaticTransforms.cpp - include/smb_estimator_graph/constants.h + include/smb_estimator_graph_ros2/constants.h ) target_link_libraries(${PROJECT_NAME} From 04be004dbb4dc47533b78daa3e19ae52a5b72ff3 Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Wed, 21 May 2025 15:34:53 +0000 Subject: [PATCH 07/60] Refactor includes to use smb_estimator_graph_ros2 namespace for consistency across files --- .../smb_estimator_graph_ros2/SmbStaticTransforms.h | 2 +- .../src/lib/SmbEstimator.cpp | 12 ++++++------ .../src/lib/SmbStaticTransforms.cpp | 6 +++--- .../smb_estimator_graph_ros2/src/lib/readParams.cpp | 8 ++++---- .../src/smb_estimator_node.cpp | 2 +- 5 files changed, 15 insertions(+), 15 deletions(-) diff --git a/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h b/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h index 6312aa56..7dde4f98 100644 --- a/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h +++ b/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h @@ -8,7 +8,7 @@ Please see the LICENSE file that has been included as part of this package. #ifndef Smb_Static_Transforms_H #define Smb_Static_Transforms_H // Workspace -#include "graph_msf_ros/extrinsics/StaticTransformsTf.h" +#include "graph_msf_ros2/extrinsics/StaticTransformsTf.h" namespace smb_se { diff --git a/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp b/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp index 75518012..b66dfdd5 100644 --- a/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp +++ b/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp @@ -6,16 +6,16 @@ Please see the LICENSE file that has been included as part of this package. */ // Implementation -#include "smb_estimator_graph/SmbEstimator.h" +#include "smb_estimator_graph_ros2/SmbEstimator.h" // Project -#include "smb_estimator_graph/SmbStaticTransforms.h" +#include "smb_estimator_graph_ros2/SmbStaticTransforms.h" // Workspace -#include "graph_msf/measurements/BinaryMeasurementXD.h" -#include "graph_msf/measurements/UnaryMeasurementXD.h" -#include "graph_msf_ros/util/conversions.h" -#include "smb_estimator_graph/constants.h" +#include "graph_msf_ros2/measurements/BinaryMeasurementXD.h" +#include "graph_msf_ros2/measurements/UnaryMeasurementXD.h" +#include "graph_msf_ros2/util/conversions.h" +#include "smb_estimator_graph_ros2/constants.h" namespace smb_se { diff --git a/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp b/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp index ef7467cb..47415885 100644 --- a/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp +++ b/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp @@ -6,15 +6,15 @@ Please see the LICENSE file that has been included as part of this package. */ // Implementation -#include "smb_estimator_graph/SmbStaticTransforms.h" +#include "smb_estimator_graph_ros2/SmbStaticTransforms.h" // ROS #include #include // Workspace -#include "graph_msf_ros/util/conversions.h" -#include "smb_estimator_graph/constants.h" +#include "graph_msf_ros2/util/conversions.h" +#include "smb_estimator_graph_ros2/constants.h" namespace smb_se { diff --git a/examples/ros2/smb_estimator_graph_ros2/src/lib/readParams.cpp b/examples/ros2/smb_estimator_graph_ros2/src/lib/readParams.cpp index a06340cf..116d6daf 100644 --- a/examples/ros2/smb_estimator_graph_ros2/src/lib/readParams.cpp +++ b/examples/ros2/smb_estimator_graph_ros2/src/lib/readParams.cpp @@ -6,14 +6,14 @@ Please see the LICENSE file that has been included as part of this package. */ // Implementation -#include "smb_estimator_graph/SmbEstimator.h" +#include "smb_estimator_graph_ros2/SmbEstimator.h" // Project -#include "smb_estimator_graph/SmbStaticTransforms.h" -#include "smb_estimator_graph/constants.h" +#include "smb_estimator_graph_ros2/SmbStaticTransforms.h" +#include "smb_estimator_graph_ros2/constants.h" // GraphMSF ROS -#include "graph_msf_ros/ros/read_ros_params.h" +#include "graph_msf_ros2/ros/read_ros_params.h" namespace smb_se { diff --git a/examples/ros2/smb_estimator_graph_ros2/src/smb_estimator_node.cpp b/examples/ros2/smb_estimator_graph_ros2/src/smb_estimator_node.cpp index 4296efae..18775d2d 100644 --- a/examples/ros2/smb_estimator_graph_ros2/src/smb_estimator_node.cpp +++ b/examples/ros2/smb_estimator_graph_ros2/src/smb_estimator_node.cpp @@ -13,7 +13,7 @@ Please see the LICENSE file that has been included as part of this package. #include // Local packages -#include "smb_estimator_graph/SmbEstimator.h" +#include "smb_estimator_graph_ros2/SmbEstimator.h" // Main node entry point int main(int argc, char** argv) { From 57fa16a702579868f4df936380af51ae6378ccf8 Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Wed, 21 May 2025 16:31:45 +0000 Subject: [PATCH 08/60] Refactor SmbEstimator and SmbStaticTransforms classes to use ROS 2 conventions, including updated includes, parameter handling, and subscription methods. Transition from ros::NodeHandle to rclcpp::Node and adjust message types accordingly for improved compatibility with ROS 2 architecture. --- .../smb_estimator_graph_ros2/SmbEstimator.h | 76 +++--- .../SmbStaticTransforms.h | 28 +- .../src/lib/SmbEstimator.cpp | 245 +++++++----------- .../src/lib/SmbStaticTransforms.cpp | 90 +++---- .../src/lib/readParams.cpp | 44 ++-- .../src/smb_estimator_node.cpp | 38 +-- 6 files changed, 230 insertions(+), 291 deletions(-) diff --git a/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbEstimator.h b/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbEstimator.h index be0cc9df..12b640dc 100644 --- a/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbEstimator.h +++ b/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbEstimator.h @@ -5,21 +5,23 @@ This file is released under the "BSD-3-Clause License". Please see the LICENSE file that has been included as part of this package. */ -#ifndef Smb_Estimator_H -#define Smb_Estimator_H +#pragma once // std #include +#include -// ROS -#include -#include -#include -#include -#include +// ROS 2 +#include +#include +#include +#include +#include +#include +#include // Workspace -#include "graph_msf_ros/GraphMsfRos.h" +#include "graph_msf_ros2/GraphMsfRos2.h" // Defined Macros #define ROS_QUEUE_SIZE 100 @@ -27,22 +29,22 @@ Please see the LICENSE file that has been included as part of this package. namespace smb_se { -class SmbEstimator : public graph_msf::GraphMsfRos { +class SmbEstimator : public graph_msf::GraphMsfRos2 { public: - SmbEstimator(std::shared_ptr privateNodePtr); - // Destructor - ~SmbEstimator() = default; - // Setup + explicit SmbEstimator(std::shared_ptr& node); + + ~SmbEstimator() override = default; + void setup(); protected: // Virtual Functions - void readParams(const ros::NodeHandle& privateNode) override; - void initializePublishers(ros::NodeHandle& privateNode) override; - void initializeSubscribers(ros::NodeHandle& privateNode) override; - void initializeMessages(ros::NodeHandle& privateNode) override; - void initializeServices(ros::NodeHandle& privateNode) override; - void imuCallback(const sensor_msgs::Imu::ConstPtr& imuPtr) override; + void readParams(); + void initializePublishers(); + void initializeSubscribers(); + void initializeMessages(); + void initializeServices(); + void imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuPtr); private: // Time @@ -76,13 +78,13 @@ class SmbEstimator : public graph_msf::GraphMsfRos { // Callbacks // LiDAR - void lidarOdometryCallback_(const nav_msgs::Odometry::ConstPtr& lidarOdomPtr); + void lidarOdometryCallback_(const nav_msgs::msg::Odometry::ConstSharedPtr& lidarOdomPtr); // Wheel Between - void wheelOdometryPoseCallback_(const nav_msgs::Odometry::ConstPtr& wheelOdomPtr); + void wheelOdometryPoseCallback_(const nav_msgs::msg::Odometry::ConstSharedPtr& wheelOdomPtr); // Wheel Linear Velocities - void wheelLinearVelocitiesCallback_(const std_msgs::Float64MultiArray::ConstPtr& wheelsSpeedsPtr); + void wheelLinearVelocitiesCallback_(const std_msgs::msg::Float64MultiArray::ConstSharedPtr& wheelsSpeedsPtr); // VIO - void vioOdometryCallback_(const nav_msgs::Odometry::ConstPtr& vioOdomPtr); + void vioOdometryCallback_(const nav_msgs::msg::Odometry::ConstSharedPtr& vioOdomPtr); // Callback Members int wheelOdometryCallbackCounter_ = -1; @@ -90,26 +92,18 @@ class SmbEstimator : public graph_msf::GraphMsfRos { double wheelOdometryTimeKm1_ = 0.0; // Subscribers - // Instances - // LIO - ros::Subscriber subLioOdometry_; - // Wheel Between - ros::Subscriber subWheelOdometryBetween_; - // Wheel Linear Velocities - ros::Subscriber subWheelLinearVelocities_; - // VIO - ros::Subscriber subVioOdometry_; - // TF Listener - tf::TransformListener tfListener_; + rclcpp::Subscription::SharedPtr subLioOdometry_; + rclcpp::Subscription::SharedPtr subWheelOdometryBetween_; + rclcpp::Subscription::SharedPtr subWheelLinearVelocities_; + rclcpp::Subscription::SharedPtr subVioOdometry_; // Publishers - // Path - ros::Publisher pubMeasMapLioPath_; - ros::Publisher pubMeasMapVioPath_; + rclcpp::Publisher::SharedPtr pubMeasMapLioPath_; + rclcpp::Publisher::SharedPtr pubMeasMapVioPath_; // Messages - nav_msgs::PathPtr measLio_mapImuPathPtr_; - nav_msgs::PathPtr measVio_mapImuPathPtr_; + std::shared_ptr measLio_mapImuPathPtr_; + std::shared_ptr measVio_mapImuPathPtr_; // Flags bool useLioOdometryFlag_ = true; @@ -120,5 +114,5 @@ class SmbEstimator : public graph_msf::GraphMsfRos { // Wheel Radius double wheelRadiusMeter_ = 0.195; }; + } // namespace smb_se -#endif // end Smb_Estimator_H diff --git a/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h b/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h index 7dde4f98..c2f52b49 100644 --- a/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h +++ b/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h @@ -5,16 +5,19 @@ This file is released under the "BSD-3-Clause License". Please see the LICENSE file that has been included as part of this package. */ -#ifndef Smb_Static_Transforms_H -#define Smb_Static_Transforms_H +#pragma once + // Workspace -#include "graph_msf_ros2/extrinsics/StaticTransformsTf.h" +#include +#include +#include namespace smb_se { class SmbStaticTransforms : public graph_msf::StaticTransformsTf { public: - SmbStaticTransforms(const std::shared_ptr privateNodePtr); + SmbStaticTransforms(const std::shared_ptr& nodePtr, + const graph_msf::StaticTransforms& staticTransforms); // Setters void setLioOdometryFrame(const std::string& s) { lidarOdometryFrame_ = s; } @@ -24,26 +27,21 @@ class SmbStaticTransforms : public graph_msf::StaticTransformsTf { void setVioOdometryFrame(const std::string& s) { vioOdometryFrame_ = s; } // Getters - const std::string& getLioOdometryFrame() { return lidarOdometryFrame_; } - const std::string& getWheelOdometryBetweenFrame() { return wheelOdometryBetweenFrame_; } - const std::string& getWheelLinearVelocityLeftFrame() { return wheelLinearVelocityLeftFrame_; } - const std::string& getWheelLinearVelocityRightFrame() { return wheelLinearVelocityRightFrame_; } - const std::string& getVioOdometryFrame() { return vioOdometryFrame_; } + const std::string& getLioOdometryFrame() const { return lidarOdometryFrame_; } + const std::string& getWheelOdometryBetweenFrame() const { return wheelOdometryBetweenFrame_; } + const std::string& getWheelLinearVelocityLeftFrame() const { return wheelLinearVelocityLeftFrame_; } + const std::string& getWheelLinearVelocityRightFrame() const { return wheelLinearVelocityRightFrame_; } + const std::string& getVioOdometryFrame() const { return vioOdometryFrame_; } private: void findTransformations() override; // Frames - // LiDAR std::string lidarOdometryFrame_; - // Wheel - // Between std::string wheelOdometryBetweenFrame_; - // Linear Velocities std::string wheelLinearVelocityLeftFrame_; std::string wheelLinearVelocityRightFrame_; - // VIO std::string vioOdometryFrame_; }; + } // namespace smb_se -#endif // end Smb_Static_Transforms_H diff --git a/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp b/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp index b66dfdd5..cd0dde7d 100644 --- a/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp +++ b/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp @@ -12,274 +12,231 @@ Please see the LICENSE file that has been included as part of this package. #include "smb_estimator_graph_ros2/SmbStaticTransforms.h" // Workspace -#include "graph_msf_ros2/measurements/BinaryMeasurementXD.h" -#include "graph_msf_ros2/measurements/UnaryMeasurementXD.h" +#include "graph_msf/measurements/BinaryMeasurementXD.h" +#include "graph_msf/measurements/UnaryMeasurementXD.h" #include "graph_msf_ros2/util/conversions.h" #include "smb_estimator_graph_ros2/constants.h" namespace smb_se { -SmbEstimator::SmbEstimator(std::shared_ptr privateNodePtr) : graph_msf::GraphMsfRos(privateNodePtr) { +SmbEstimator::SmbEstimator(std::shared_ptr& node) : graph_msf::GraphMsfRos2(node) { REGULAR_COUT << GREEN_START << " SmbEstimator-Constructor called." << COLOR_END << std::endl; - // Configurations ---------------------------- - // Static transforms - staticTransformsPtr_ = std::make_shared(privateNodePtr); - - // Set up + staticTransformsPtr_ = std::make_shared(node_); + // Call setup after declaring parameters SmbEstimator::setup(); } void SmbEstimator::setup() { REGULAR_COUT << GREEN_START << " SmbEstimator-Setup called." << COLOR_END << std::endl; - // Read parameters ---------------------------- - SmbEstimator::readParams(privateNode); - - // Super class - GraphMsfRos::setup(staticTransformsPtr_); - - // Publishers ---------------------------- - SmbEstimator::initializePublishers(privateNode); - - // Subscribers ---------------------------- - SmbEstimator::initializeSubscribers(privateNode); - - // Messages ---------------------------- - SmbEstimator::initializeMessages(privateNode); - - // Services ---------------------------- - SmbEstimator::initializeServices(privateNode); - - // Transforms ---------------------------- + // Boolean flags + node_->declare_parameter("sensor_params.useLioOdometry", false); + node_->declare_parameter("sensor_params.useWheelOdometryBetween", false); + node_->declare_parameter("sensor_params.useWheelLinearVelocities", false); + node_->declare_parameter("sensor_params.useVioOdometry", false); + + // Sensor parameters (int) + node_->declare_parameter("sensor_params.lioOdometryRate", 0); + node_->declare_parameter("sensor_params.wheelOdometryBetweenRate", 0); + node_->declare_parameter("sensor_params.wheelLinearVelocitiesRate", 0); + node_->declare_parameter("sensor_params.vioOdometryRate", 0); + + // Alignment parameters (vector of double) + node_->declare_parameter("alignment_params.initialSe3AlignmentNoiseDensity", std::vector{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); + node_->declare_parameter("alignment_params.lioSe3AlignmentRandomWalk", std::vector{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); + + // Noise parameters (vectors of double) + node_->declare_parameter("noise_params.lioPoseUnaryNoiseDensity", std::vector{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); + node_->declare_parameter("noise_params.wheelPoseBetweenNoiseDensity", std::vector{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); + node_->declare_parameter("noise_params.wheelLinearVelocitiesNoiseDensity", std::vector{0.0, 0.0, 0.0}); + node_->declare_parameter("noise_params.vioPoseBetweenNoiseDensity", std::vector{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); + + // Extrinsic frames (string) + node_->declare_parameter("extrinsics.lidarOdometryFrame", std::string("")); + node_->declare_parameter("extrinsics.wheelOdometryBetweenFrame", std::string("")); + node_->declare_parameter("extrinsics.wheelLinearVelocityLeftFrame", std::string("")); + node_->declare_parameter("extrinsics.wheelLinearVelocityRightFrame", std::string("")); + node_->declare_parameter("extrinsics.vioOdometryFrame", std::string("")); + + // Wheel Radius (double) + node_->declare_parameter("sensor_params.wheelRadius", 0.0); + + SmbEstimator::readParams(); + + // Initialize ROS 2 publishers and subscribers + SmbEstimator::initializePublishers(); + SmbEstimator::initializeSubscribers(); + SmbEstimator::initializeMessages(); + SmbEstimator::initializeServices(); + + GraphMsfRos2::setup(staticTransformsPtr_); + + // Transforms staticTransformsPtr_->findTransformations(); - // Wrap up ---------------------------- REGULAR_COUT << GREEN_START << " Set up successfully." << COLOR_END << std::endl; } -void SmbEstimator::initializePublishers(ros::NodeHandle& privateNode) { - // Paths - pubMeasMapLioPath_ = privateNode.advertise("/graph_msf/measLiDAR_path_map_imu", ROS_QUEUE_SIZE); - pubMeasMapVioPath_ = privateNode.advertise("/graph_msf/measVIO_path_map_imu", ROS_QUEUE_SIZE); +void SmbEstimator::initializePublishers() { + pubMeasMapLioPath_ = node_->create_publisher("/graph_msf/measLiDAR_path_map_imu", ROS_QUEUE_SIZE); + pubMeasMapVioPath_ = node_->create_publisher("/graph_msf/measVIO_path_map_imu", ROS_QUEUE_SIZE); } -void SmbEstimator::initializeSubscribers(ros::NodeHandle& privateNode) { - // LiDAR Odometry +void SmbEstimator::initializeSubscribers() { if (useLioOdometryFlag_) { - subLioOdometry_ = privateNode.subscribe( - "/lidar_odometry_topic", ROS_QUEUE_SIZE, &SmbEstimator::lidarOdometryCallback_, this, ros::TransportHints().tcpNoDelay()); - REGULAR_COUT << COLOR_END << " Initialized LiDAR Odometry subscriber with topic: " << subLioOdometry_.getTopic() << std::endl; + subLioOdometry_ = node_->create_subscription( + "/lidar_odometry_topic", ROS_QUEUE_SIZE, std::bind(&SmbEstimator::lidarOdometryCallback_, this, std::placeholders::_1)); + REGULAR_COUT << COLOR_END << " Initialized LiDAR Odometry subscriber with topic: /lidar_odometry_topic" << std::endl; } - // Wheel Odometry Between if (useWheelOdometryBetweenFlag_) { - subWheelOdometryBetween_ = privateNode.subscribe( - "/wheel_odometry_topic", ROS_QUEUE_SIZE, &SmbEstimator::wheelOdometryPoseCallback_, this, ros::TransportHints().tcpNoDelay()); - REGULAR_COUT << COLOR_END << " Initialized Wheel Odometry subscriber with topic: " << subWheelOdometryBetween_.getTopic() << std::endl; + subWheelOdometryBetween_ = node_->create_subscription( + "/wheel_odometry_topic", ROS_QUEUE_SIZE, std::bind(&SmbEstimator::wheelOdometryPoseCallback_, this, std::placeholders::_1)); + REGULAR_COUT << COLOR_END << " Initialized Wheel Odometry subscriber with topic: /wheel_odometry_topic" << std::endl; } - // Wheel Linear Velocities if (useWheelLinearVelocitiesFlag_) { - subWheelLinearVelocities_ = privateNode.subscribe( - "/wheel_velocities_topic", ROS_QUEUE_SIZE, &SmbEstimator::wheelLinearVelocitiesCallback_, this, ros::TransportHints().tcpNoDelay()); - REGULAR_COUT << COLOR_END << " Initialized Wheel Linear Velocities subscriber with topic: " << subWheelLinearVelocities_.getTopic() - << std::endl; + subWheelLinearVelocities_ = node_->create_subscription( + "/wheel_velocities_topic", ROS_QUEUE_SIZE, std::bind(&SmbEstimator::wheelLinearVelocitiesCallback_, this, std::placeholders::_1)); + REGULAR_COUT << COLOR_END << " Initialized Wheel Linear Velocities subscriber with topic: /wheel_velocities_topic" << std::endl; } - // VIO if (useVioOdometryFlag_) { - subVioOdometry_ = privateNode.subscribe("/vio_odometry_topic", ROS_QUEUE_SIZE, &SmbEstimator::vioOdometryCallback_, - this, ros::TransportHints().tcpNoDelay()); - REGULAR_COUT << COLOR_END << " Initialized VIO Odometry subscriber with topic: " << subVioOdometry_.getTopic() << std::endl; + subVioOdometry_ = node_->create_subscription( + "/vio_odometry_topic", ROS_QUEUE_SIZE, std::bind(&SmbEstimator::vioOdometryCallback_, this, std::placeholders::_1)); + REGULAR_COUT << COLOR_END << " Initialized VIO Odometry subscriber with topic: /vio_odometry_topic" << std::endl; } } -void SmbEstimator::initializeMessages(ros::NodeHandle& privateNode) { - // Path - measLio_mapImuPathPtr_ = nav_msgs::PathPtr(new nav_msgs::Path); - measVio_mapImuPathPtr_ = nav_msgs::PathPtr(new nav_msgs::Path); +void SmbEstimator::initializeMessages() { + measLio_mapImuPathPtr_ = std::make_shared(); + measVio_mapImuPathPtr_ = std::make_shared(); } -void SmbEstimator::initializeServices(ros::NodeHandle& privateNode) { +void SmbEstimator::initializeServices() { // Nothing for now } -void SmbEstimator::imuCallback(const sensor_msgs::Imu::ConstPtr& imuPtr) { - // Check whether any of the measurements is available, otherwise do pure imu integration +void SmbEstimator::imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuPtr) { if (graph_msf::GraphMsf::areRollAndPitchInited() && !graph_msf::GraphMsf::areYawAndPositionInited() && !useLioOdometryFlag_ && !useWheelOdometryBetweenFlag_ && !useWheelLinearVelocitiesFlag_ && !useVioOdometryFlag_) { - // Initialization REGULAR_COUT << RED_START << " IMU callback is setting global yaw and position, as no other odometry is available. Initializing..." << COLOR_END << std::endl; - // Create dummy measurement for initialization + graph_msf::UnaryMeasurementXD unary6DMeasurement( "IMU_init_6D", int(graphConfigPtr_->imuRate_), staticTransformsPtr_->getImuFrame(), - staticTransformsPtr_->getImuFrame() + sensorFrameCorrectedNameId, graph_msf::RobustNorm::None(), imuPtr->header.stamp.toSec(), 1.0, - Eigen::Isometry3d::Identity(), Eigen::MatrixXd::Identity(6, 1)); - // Initialize + staticTransformsPtr_->getImuFrame() + sensorFrameCorrectedNameId, graph_msf::RobustNorm::None(), + imuPtr->header.stamp.sec + imuPtr->header.stamp.nanosec * 1e-9, 1.0, Eigen::Isometry3d::Identity(), + Eigen::MatrixXd::Identity(6, 1)); + graph_msf::GraphMsf::initYawAndPosition(unary6DMeasurement); - REGULAR_COUT << RED_START << " ...initialized yaw and position to identity." << COLOR_END << std::endl; - // Pretend that we received first measurement --> In order to allow for optimization graph_msf::GraphMsf::pretendFirstMeasurementReceived(); } - // Super class - graph_msf::GraphMsfRos::imuCallback(imuPtr); + graph_msf::GraphMsfRos2::imuCallback(imuPtr); } -void SmbEstimator::lidarOdometryCallback_(const nav_msgs::Odometry::ConstPtr& odomLidarPtr) { - // Static members +void SmbEstimator::lidarOdometryCallback_(const nav_msgs::msg::Odometry::ConstSharedPtr& odomLidarPtr) { static int lidarOdometryCallbackCounter__ = -1; - - // Counter ++lidarOdometryCallbackCounter__; Eigen::Isometry3d lio_T_M_Lk; graph_msf::odomMsgToEigen(*odomLidarPtr, lio_T_M_Lk.matrix()); + double lidarOdometryTimeK = odomLidarPtr->header.stamp.sec + odomLidarPtr->header.stamp.nanosec * 1e-9; - // Transform to IMU frame - double lidarOdometryTimeK = odomLidarPtr->header.stamp.toSec(); - - // Frame Name - const std::string& lioOdometryFrame = dynamic_cast(staticTransformsPtr_.get())->getLioOdometryFrame(); // alias + const std::string& lioOdometryFrame = dynamic_cast(staticTransformsPtr_.get())->getLioOdometryFrame(); - // Measurement graph_msf::UnaryMeasurementXDAbsolute unary6DMeasurement( "Lidar_unary_6D", int(lioOdometryRate_), lioOdometryFrame, lioOdometryFrame + sensorFrameCorrectedNameId, graph_msf::RobustNorm::None(), lidarOdometryTimeK, 1.0, lio_T_M_Lk, lioPoseUnaryNoise_, odomLidarPtr->header.frame_id, staticTransformsPtr_->getWorldFrame(), initialSe3AlignmentNoise_, lioSe3AlignmentRandomWalk_); - // Add measurement or initialize if (lidarOdometryCallbackCounter__ <= 2) { return; - } else if (areYawAndPositionInited()) { // Already initialized --> unary factor + } else if (areYawAndPositionInited()) { this->addUnaryPose3AbsoluteMeasurement(unary6DMeasurement); - } else { // Initializing - REGULAR_COUT << GREEN_START << " LiDAR odometry callback is setting global yaw, as it was not set so far." << COLOR_END << std::endl; + } else { this->initYawAndPosition(unary6DMeasurement); } - // Visualization ---------------------------- - // Add to path message addToPathMsg(measLio_mapImuPathPtr_, odomLidarPtr->header.frame_id, odomLidarPtr->header.stamp, - (lio_T_M_Lk * staticTransformsPtr_ - ->rv_T_frame1_frame2(dynamic_cast(staticTransformsPtr_.get())->getLioOdometryFrame(), - staticTransformsPtr_->getImuFrame()) - .matrix()) + (lio_T_M_Lk * staticTransformsPtr_->rv_T_frame1_frame2(lioOdometryFrame, staticTransformsPtr_->getImuFrame()).matrix()) .block<3, 1>(0, 3), graphConfigPtr_->imuBufferLength_ * 4); - // Publish Path - pubMeasMapLioPath_.publish(measLio_mapImuPathPtr_); + pubMeasMapLioPath_->publish(*measLio_mapImuPathPtr_); } -void SmbEstimator::wheelOdometryPoseCallback_(const nav_msgs::Odometry::ConstPtr& wheelOdometryKPtr) { +void SmbEstimator::wheelOdometryPoseCallback_(const nav_msgs::msg::Odometry::ConstSharedPtr& wheelOdometryKPtr) { if (!areRollAndPitchInited()) { return; } - // Counter ++wheelOdometryCallbackCounter_; - // Eigen Type Eigen::Isometry3d T_O_Bw_k; graph_msf::odomMsgToEigen(*wheelOdometryKPtr, T_O_Bw_k.matrix()); - double wheelOdometryTimeK = wheelOdometryKPtr->header.stamp.toSec(); + double wheelOdometryTimeK = wheelOdometryKPtr->header.stamp.sec + wheelOdometryKPtr->header.stamp.nanosec * 1e-9; - // At start if (wheelOdometryCallbackCounter_ == 0) { T_O_Bw_km1_ = T_O_Bw_k; wheelOdometryTimeKm1_ = wheelOdometryTimeK; return; } - // Frame name - const std::string& wheelOdometryFrame = - dynamic_cast(staticTransformsPtr_.get())->getWheelOdometryBetweenFrame(); // alias + const std::string& wheelOdometryFrame = dynamic_cast(staticTransformsPtr_.get())->getWheelOdometryBetweenFrame(); - // State Machine if (!areYawAndPositionInited()) { - // If no LIO, then initialize the graph here if (!useLioOdometryFlag_) { - REGULAR_COUT << GREEN_START << " Wheel odometry callback is setting global yaw and position, as lio is all set to false." << COLOR_END - << std::endl; graph_msf::UnaryMeasurementXD unary6DMeasurement( "Lidar_unary_6D", int(wheelOdometryBetweenRate_), wheelOdometryFrame, wheelOdometryFrame + sensorFrameCorrectedNameId, graph_msf::RobustNorm::None(), wheelOdometryTimeK, 1.0, Eigen::Isometry3d::Identity(), Eigen::MatrixXd::Identity(6, 1)); graph_msf::GraphMsf::initYawAndPosition(unary6DMeasurement); - REGULAR_COUT << " Initialized yaw and position to identity in the wheel odometry callback, as lio and vio are all set to false." - << std::endl; - } - } else { - // Only add every 5th measurement - int measurementRate = static_cast(wheelOdometryBetweenRate_ / 5); - // Check - if (wheelOdometryCallbackCounter_ % 5 == 0 && wheelOdometryCallbackCounter_ > 0) { - // Compute Delta - Eigen::Isometry3d T_Bkm1_Bk = T_O_Bw_km1_.inverse() * T_O_Bw_k; - // Create measurement - graph_msf::BinaryMeasurementXD delta6DMeasurement( - "Wheel_odometry_6D", measurementRate, wheelOdometryFrame, wheelOdometryFrame + sensorFrameCorrectedNameId, - graph_msf::RobustNorm::Tukey(1.0), wheelOdometryTimeKm1_, wheelOdometryTimeK, T_Bkm1_Bk, wheelPoseBetweenNoise_); - // Add to graph - this->addBinaryPose3Measurement(delta6DMeasurement); - - // Prepare for next iteration - T_O_Bw_km1_ = T_O_Bw_k; - wheelOdometryTimeKm1_ = wheelOdometryTimeK; } + } else if (wheelOdometryCallbackCounter_ % 5 == 0 && wheelOdometryCallbackCounter_ > 0) { + Eigen::Isometry3d T_Bkm1_Bk = T_O_Bw_km1_.inverse() * T_O_Bw_k; + graph_msf::BinaryMeasurementXD delta6DMeasurement( + "Wheel_odometry_6D", int(wheelOdometryBetweenRate_ / 5), wheelOdometryFrame, wheelOdometryFrame + sensorFrameCorrectedNameId, + graph_msf::RobustNorm::Tukey(1.0), wheelOdometryTimeKm1_, wheelOdometryTimeK, T_Bkm1_Bk, wheelPoseBetweenNoise_); + this->addBinaryPose3Measurement(delta6DMeasurement); + + T_O_Bw_km1_ = T_O_Bw_k; + wheelOdometryTimeKm1_ = wheelOdometryTimeK; } } -void SmbEstimator::wheelLinearVelocitiesCallback_(const std_msgs::Float64MultiArray::ConstPtr& wheelsSpeedsPtr) { +void SmbEstimator::wheelLinearVelocitiesCallback_(const std_msgs::msg::Float64MultiArray::ConstSharedPtr& wheelsSpeedsPtr) { if (!areRollAndPitchInited()) { return; } - // Get the 3 values const double timeK = wheelsSpeedsPtr->data[0]; const double leftWheelSpeedRps = wheelsSpeedsPtr->data[1]; const double rightWheelSpeedRps = wheelsSpeedsPtr->data[2]; const double leftWheelSpeedMs = leftWheelSpeedRps * wheelRadiusMeter_; const double rightWheelSpeedMs = rightWheelSpeedRps * wheelRadiusMeter_; - // Print - if (graphConfigPtr_->verboseLevel_ > 1) { - REGULAR_COUT << " Wheel linear velocities callback: " << std::setprecision(14) << timeK << " " << leftWheelSpeedMs << " [m/s] " - << rightWheelSpeedMs << " [m/s]" << std::endl; - } - - // Frame name const std::string& wheelLinearVelocityLeftFrame = - dynamic_cast(staticTransformsPtr_.get())->getWheelLinearVelocityLeftFrame(); // alias + dynamic_cast(staticTransformsPtr_.get())->getWheelLinearVelocityLeftFrame(); const std::string& wheelLinearVelocityRightFrame = - dynamic_cast(staticTransformsPtr_.get())->getWheelLinearVelocityRightFrame(); // alias + dynamic_cast(staticTransformsPtr_.get())->getWheelLinearVelocityRightFrame(); - // State Machine if (!areYawAndPositionInited()) { - // If no LIO, then initialize the graph here if (!useLioOdometryFlag_ && !useWheelOdometryBetweenFlag_) { - REGULAR_COUT << GREEN_START << " Wheel linear velocities callback is setting global yaw and position, as lio is all set to false." - << COLOR_END << std::endl; graph_msf::UnaryMeasurementXD unary6DMeasurement( "Lidar_unary_6D", int(wheelLinearVelocitiesRate_), wheelLinearVelocityLeftFrame, wheelLinearVelocityLeftFrame + sensorFrameCorrectedNameId, graph_msf::RobustNorm::None(), timeK, 1.0, Eigen::Isometry3d::Identity(), Eigen::MatrixXd::Identity(6, 1)); graph_msf::GraphMsf::initYawAndPosition(unary6DMeasurement); - REGULAR_COUT - << " Initialized yaw and position to identity in the wheel linear velocities callback, as lio and vio are all set to false." - << std::endl; } } else { - // Left Wheel graph_msf::UnaryMeasurementXD leftWheelLinearVelocityMeasurement( "Wheel_linear_velocity_left", int(wheelLinearVelocitiesRate_), wheelLinearVelocityLeftFrame, wheelLinearVelocityLeftFrame + sensorFrameCorrectedNameId, graph_msf::RobustNorm::Tukey(1.0), timeK, 1.0, Eigen::Vector3d(leftWheelSpeedMs, 0.0, 0.0), wheelLinearVelocitiesNoise_); this->addUnaryVelocity3LocalMeasurement(leftWheelLinearVelocityMeasurement); - // Right Wheel graph_msf::UnaryMeasurementXD rightWheelLinearVelocityMeasurement( "Wheel_linear_velocity_right", int(wheelLinearVelocitiesRate_), wheelLinearVelocityRightFrame, wheelLinearVelocityRightFrame + sensorFrameCorrectedNameId, graph_msf::RobustNorm::Tukey(1.0), timeK, 1.0, @@ -288,15 +245,12 @@ void SmbEstimator::wheelLinearVelocitiesCallback_(const std_msgs::Float64MultiAr } } -void SmbEstimator::vioOdometryCallback_(const nav_msgs::Odometry::ConstPtr& vioOdomPtr) { +void SmbEstimator::vioOdometryCallback_(const nav_msgs::msg::Odometry::ConstSharedPtr& vioOdomPtr) { std::cout << "VIO odometry not yet stable enough for usage, disable flag." << std::endl; - // Extract Eigen::Isometry3d vio_T_M_Ck; graph_msf::odomMsgToEigen(*vioOdomPtr, vio_T_M_Ck.matrix()); - // Visualization ---------------------------- - // Add to path message addToPathMsg(measVio_mapImuPathPtr_, vioOdomPtr->header.frame_id, vioOdomPtr->header.stamp, (vio_T_M_Ck * staticTransformsPtr_ ->rv_T_frame1_frame2(dynamic_cast(staticTransformsPtr_.get())->getVioOdometryFrame(), @@ -305,8 +259,7 @@ void SmbEstimator::vioOdometryCallback_(const nav_msgs::Odometry::ConstPtr& vioO .block<3, 1>(0, 3), graphConfigPtr_->imuBufferLength_ * 4 * 10); - // Publish Path - pubMeasMapVioPath_.publish(measVio_mapImuPathPtr_); + pubMeasMapVioPath_->publish(*measVio_mapImuPathPtr_); } } // namespace smb_se \ No newline at end of file diff --git a/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp b/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp index 47415885..8ff03fa2 100644 --- a/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp +++ b/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp @@ -8,9 +8,10 @@ Please see the LICENSE file that has been included as part of this package. // Implementation #include "smb_estimator_graph_ros2/SmbStaticTransforms.h" -// ROS -#include -#include +// ROS 2 +#include +#include +#include // Workspace #include "graph_msf_ros2/util/conversions.h" @@ -18,7 +19,9 @@ Please see the LICENSE file that has been included as part of this package. namespace smb_se { -SmbStaticTransforms::SmbStaticTransforms(const std::shared_ptr privateNodePtr) : graph_msf::StaticTransformsTf() { +SmbStaticTransforms::SmbStaticTransforms(const std::shared_ptr& nodePtr, + const graph_msf::StaticTransforms& staticTransforms) + : graph_msf::StaticTransformsTf(nodePtr, staticTransforms) { REGULAR_COUT << GREEN_START << " Initializing static transforms..." << COLOR_END << std::endl; } @@ -26,74 +29,61 @@ void SmbStaticTransforms::findTransformations() { // Super Method graph_msf::StaticTransformsTf::findTransformations(); - // Print to console -------------------------- + // Print to console REGULAR_COUT << COLOR_END << " Looking up transforms in TF-tree." << std::endl; REGULAR_COUT << COLOR_END << " Transforms between the following frames are required:" << std::endl; REGULAR_COUT << COLOR_END << " Waiting for up to 100 seconds until they arrive..." << std::endl; // Temporary variable - static tf::StampedTransform transform; - - // Look up transforms ---------------------------- - ros::Rate rosRate(10); - rosRate.sleep(); + geometry_msgs::msg::TransformStamped transform; // Imu to LiDAR Link --- - REGULAR_COUT << COLOR_END << " Waiting for transform for 10 seconds."; - listener_.waitForTransform(imuFrame_, lidarOdometryFrame_, ros::Time(0), ros::Duration(1.0)); - listener_.lookupTransform(imuFrame_, lidarOdometryFrame_, ros::Time(0), transform); - // I_Lidar - graph_msf::tfToIsometry3(tf::Transform(transform), lv_T_frame1_frame2(imuFrame_, lidarOdometryFrame_)); - std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END - << " Translation I_Lidar: " << rv_T_frame1_frame2(imuFrame_, lidarOdometryFrame_).translation() << std::endl; - // Lidar_I + REGULAR_COUT << COLOR_END << " Waiting for transform LO for 10 seconds." << std::endl; + transform = tf_buffer_->lookupTransform(imuFrame_, lidarOdometryFrame_, tf2::TimePointZero, tf2::durationFromSec(1.0)); + Eigen::Isometry3d eigenTransform = tf2::transformToEigen(transform.transform); + lv_T_frame1_frame2(imuFrame_, lidarOdometryFrame_) = eigenTransform; + + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END << " Translation I_Lidar: " << imuFrame_ << " " << lidarOdometryFrame_ + << " " << rv_T_frame1_frame2(imuFrame_, lidarOdometryFrame_).translation() << std::endl; lv_T_frame1_frame2(lidarOdometryFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, lidarOdometryFrame_).inverse(); // Imu to VIO Link --- - REGULAR_COUT << COLOR_END << " Waiting for transform for 10 seconds."; - listener_.waitForTransform(imuFrame_, vioOdometryFrame_, ros::Time(0), ros::Duration(1.0)); - listener_.lookupTransform(imuFrame_, vioOdometryFrame_, ros::Time(0), transform); - // I_VIO - graph_msf::tfToIsometry3(tf::Transform(transform), lv_T_frame1_frame2(imuFrame_, vioOdometryFrame_)); - std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END - << " Translation I_VIO: " << rv_T_frame1_frame2(imuFrame_, vioOdometryFrame_).translation() << std::endl; - // VIO_I + REGULAR_COUT << COLOR_END << " Waiting for transform VIO for 10 seconds." << std::endl; + transform = tf_buffer_->lookupTransform(imuFrame_, vioOdometryFrame_, tf2::TimePointZero, tf2::durationFromSec(1.0)); + eigenTransform = tf2::transformToEigen(transform.transform); + lv_T_frame1_frame2(imuFrame_, vioOdometryFrame_) = eigenTransform; + + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END << " Translation I_VIO: " << imuFrame_ << " " << vioOdometryFrame_ << " " + << rv_T_frame1_frame2(imuFrame_, vioOdometryFrame_).translation() << std::endl; lv_T_frame1_frame2(vioOdometryFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, vioOdometryFrame_).inverse(); // Wheel Frames --- - REGULAR_COUT - << RED_START - << " As the Wheels are turning, we only use the position of the wheels frames and use the orientation of the baseLinkFrame_ frame." - << COLOR_END << std::endl; + REGULAR_COUT << RED_START + << " As the Wheels are turning, we only use the position of the wheels frames and use the orientation of the baseLinkFrame_ frame." + << COLOR_END << std::endl; + // Left Wheel - REGULAR_COUT << COLOR_END << " Waiting for transform for 10 seconds."; - listener_.waitForTransform(imuFrame_, wheelLinearVelocityLeftFrame_, ros::Time(0), ros::Duration(1.0)); - listener_.lookupTransform(imuFrame_, wheelLinearVelocityLeftFrame_, ros::Time(0), transform); - // I_WheelLeft - Eigen::Isometry3d T_I_WheelLeft; - graph_msf::tfToIsometry3(tf::Transform(transform), T_I_WheelLeft); - // Overwrite the orientation with the one of the baseLinkFrame_ + REGULAR_COUT << COLOR_END << " Waiting for transform for 10 seconds." << std::endl; + transform = tf_buffer_->lookupTransform(imuFrame_, wheelLinearVelocityLeftFrame_, tf2::TimePointZero, tf2::durationFromSec(1.0)); + Eigen::Isometry3d T_I_WheelLeft = tf2::transformToEigen(transform.transform); T_I_WheelLeft.matrix().block<3, 3>(0, 0) = rv_T_frame1_frame2(imuFrame_, baseLinkFrame_).matrix().block<3, 3>(0, 0); lv_T_frame1_frame2(imuFrame_, wheelLinearVelocityLeftFrame_) = T_I_WheelLeft; - std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END - << " Translation I_WheelLeft: " << rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityLeftFrame_).translation() + + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END << " Translation I_WheelLeft: " + << rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityLeftFrame_).translation() << ", Rotation: " << T_I_WheelLeft.matrix().block<3, 3>(0, 0) << std::endl; - // WheelLeft_I lv_T_frame1_frame2(wheelLinearVelocityLeftFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityLeftFrame_).inverse(); + // Right Wheel - REGULAR_COUT << COLOR_END << " Waiting for transform for 10 seconds."; - listener_.waitForTransform(imuFrame_, wheelLinearVelocityRightFrame_, ros::Time(0), ros::Duration(1.0)); - listener_.lookupTransform(imuFrame_, wheelLinearVelocityRightFrame_, ros::Time(0), transform); - // I_WheelRight - Eigen::Isometry3d T_I_WheelRight; - graph_msf::tfToIsometry3(tf::Transform(transform), T_I_WheelRight); - // Overwrite the orientation with the one of the baseLinkFrame_ + REGULAR_COUT << COLOR_END << " Waiting for transform for 10 seconds." << std::endl; + transform = tf_buffer_->lookupTransform(imuFrame_, wheelLinearVelocityRightFrame_, tf2::TimePointZero, tf2::durationFromSec(1.0)); + Eigen::Isometry3d T_I_WheelRight = tf2::transformToEigen(transform.transform); T_I_WheelRight.matrix().block<3, 3>(0, 0) = rv_T_frame1_frame2(imuFrame_, baseLinkFrame_).matrix().block<3, 3>(0, 0); lv_T_frame1_frame2(imuFrame_, wheelLinearVelocityRightFrame_) = T_I_WheelRight; - std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END - << " Translation I_WheelRight: " << rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityRightFrame_).translation() + + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END << " Translation I_WheelRight: " + << rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityRightFrame_).translation() << ", Rotation: " << T_I_WheelRight.matrix().block<3, 3>(0, 0) << std::endl; - // WheelRight_I lv_T_frame1_frame2(wheelLinearVelocityRightFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityRightFrame_).inverse(); REGULAR_COUT << GREEN_START << " Transforms looked up successfully." << COLOR_END << std::endl; diff --git a/examples/ros2/smb_estimator_graph_ros2/src/lib/readParams.cpp b/examples/ros2/smb_estimator_graph_ros2/src/lib/readParams.cpp index 116d6daf..7064d0e6 100644 --- a/examples/ros2/smb_estimator_graph_ros2/src/lib/readParams.cpp +++ b/examples/ros2/smb_estimator_graph_ros2/src/lib/readParams.cpp @@ -12,81 +12,81 @@ Please see the LICENSE file that has been included as part of this package. #include "smb_estimator_graph_ros2/SmbStaticTransforms.h" #include "smb_estimator_graph_ros2/constants.h" -// GraphMSF ROS +// GraphMSF ROS2 #include "graph_msf_ros2/ros/read_ros_params.h" namespace smb_se { -void SmbEstimator::readParams(const ros::NodeHandle& privateNode) { +void SmbEstimator::readParams() { // Check if (!graphConfigPtr_) { throw std::runtime_error("SmbEstimator: graphConfigPtr must be initialized."); } // Flags - useLioOdometryFlag_ = graph_msf::tryGetParam("sensor_params/useLioOdometry", privateNode); - useWheelOdometryBetweenFlag_ = graph_msf::tryGetParam("sensor_params/useWheelOdometryBetween", privateNode); - useWheelLinearVelocitiesFlag_ = graph_msf::tryGetParam("sensor_params/useWheelLinearVelocities", privateNode); - useVioOdometryFlag_ = graph_msf::tryGetParam("sensor_params/useVioOdometry", privateNode); + useLioOdometryFlag_ = graph_msf::tryGetParam(node_, "sensor_params.useLioOdometry"); + useWheelOdometryBetweenFlag_ = graph_msf::tryGetParam(node_, "sensor_params.useWheelOdometryBetween"); + useWheelLinearVelocitiesFlag_ = graph_msf::tryGetParam(node_, "sensor_params.useWheelLinearVelocities"); + useVioOdometryFlag_ = graph_msf::tryGetParam(node_, "sensor_params.useVioOdometry"); // Sensor Params - lioOdometryRate_ = graph_msf::tryGetParam("sensor_params/lioOdometryRate", privateNode); - wheelOdometryBetweenRate_ = graph_msf::tryGetParam("sensor_params/wheelOdometryBetweenRate", privateNode); - wheelLinearVelocitiesRate_ = graph_msf::tryGetParam("sensor_params/wheelLinearVelocitiesRate", privateNode); - vioOdometryRate_ = graph_msf::tryGetParam("sensor_params/vioOdometryRate", privateNode); + lioOdometryRate_ = graph_msf::tryGetParam(node_, "sensor_params.lioOdometryRate"); + wheelOdometryBetweenRate_ = graph_msf::tryGetParam(node_, "sensor_params.wheelOdometryBetweenRate"); + wheelLinearVelocitiesRate_ = graph_msf::tryGetParam(node_, "sensor_params.wheelLinearVelocitiesRate"); + vioOdometryRate_ = graph_msf::tryGetParam(node_, "sensor_params.vioOdometryRate"); // Alignment Parameters const auto initialSe3AlignmentNoiseDensity = - graph_msf::tryGetParam>("alignment_params/initialSe3AlignmentNoiseDensity", privateNode); + graph_msf::tryGetParam>(node_, "alignment_params.initialSe3AlignmentNoiseDensity"); initialSe3AlignmentNoise_ << initialSe3AlignmentNoiseDensity[0], initialSe3AlignmentNoiseDensity[1], initialSe3AlignmentNoiseDensity[2], initialSe3AlignmentNoiseDensity[3], initialSe3AlignmentNoiseDensity[4], initialSe3AlignmentNoiseDensity[5]; const auto lioSe3AlignmentRandomWalk = - graph_msf::tryGetParam>("alignment_params/lioSe3AlignmentRandomWalk", privateNode); + graph_msf::tryGetParam>(node_, "alignment_params.lioSe3AlignmentRandomWalk"); lioSe3AlignmentRandomWalk_ << lioSe3AlignmentRandomWalk[0], lioSe3AlignmentRandomWalk[1], lioSe3AlignmentRandomWalk[2], lioSe3AlignmentRandomWalk[3], lioSe3AlignmentRandomWalk[4], lioSe3AlignmentRandomWalk[5]; // Noise Parameters /// LiDAR Odometry const auto poseUnaryNoise = - graph_msf::tryGetParam>("noise_params/lioPoseUnaryNoiseDensity", privateNode); // roll,pitch,yaw,x,y,z + graph_msf::tryGetParam>(node_, "noise_params.lioPoseUnaryNoiseDensity"); // roll,pitch,yaw,x,y,z lioPoseUnaryNoise_ << poseUnaryNoise[0], poseUnaryNoise[1], poseUnaryNoise[2], poseUnaryNoise[3], poseUnaryNoise[4], poseUnaryNoise[5]; /// Wheel Odometry /// Between const auto wheelPoseBetweenNoise = - graph_msf::tryGetParam>("noise_params/wheelPoseBetweenNoiseDensity", privateNode); // roll,pitch,yaw,x,y,z + graph_msf::tryGetParam>(node_, "noise_params.wheelPoseBetweenNoiseDensity"); // roll,pitch,yaw,x,y,z wheelPoseBetweenNoise_ << wheelPoseBetweenNoise[0], wheelPoseBetweenNoise[1], wheelPoseBetweenNoise[2], wheelPoseBetweenNoise[3], wheelPoseBetweenNoise[4], wheelPoseBetweenNoise[5]; /// Linear Velocities const auto wheelLinearVelocitiesNoise = - graph_msf::tryGetParam>("noise_params/wheelLinearVelocitiesNoiseDensity", privateNode); // left,right + graph_msf::tryGetParam>(node_, "noise_params.wheelLinearVelocitiesNoiseDensity"); // left,right wheelLinearVelocitiesNoise_ << wheelLinearVelocitiesNoise[0], wheelLinearVelocitiesNoise[1], wheelLinearVelocitiesNoise[2]; /// VIO Odometry const auto vioPoseBetweenNoise = - graph_msf::tryGetParam>("noise_params/vioPoseBetweenNoiseDensity", privateNode); // roll,pitch,yaw,x,y,z + graph_msf::tryGetParam>(node_, "noise_params.vioPoseBetweenNoiseDensity"); // roll,pitch,yaw,x,y,z vioPoseBetweenNoise_ << vioPoseBetweenNoise[0], vioPoseBetweenNoise[1], vioPoseBetweenNoise[2], vioPoseBetweenNoise[3], vioPoseBetweenNoise[4], vioPoseBetweenNoise[5]; // Set frames /// LiDAR odometry frame dynamic_cast(staticTransformsPtr_.get()) - ->setLioOdometryFrame(graph_msf::tryGetParam("extrinsics/lidarOdometryFrame", privateNode)); + ->setLioOdometryFrame(graph_msf::tryGetParam(node_, "extrinsics.lidarOdometryFrame")); /// Wheel Odometry frame dynamic_cast(staticTransformsPtr_.get()) - ->setWheelOdometryBetweenFrame(graph_msf::tryGetParam("extrinsics/wheelOdometryBetweenFrame", privateNode)); + ->setWheelOdometryBetweenFrame(graph_msf::tryGetParam(node_, "extrinsics.wheelOdometryBetweenFrame")); /// Whel Linear Velocities frames /// Left dynamic_cast(staticTransformsPtr_.get()) - ->setWheelLinearVelocityLeftFrame(graph_msf::tryGetParam("extrinsics/wheelLinearVelocityLeftFrame", privateNode)); + ->setWheelLinearVelocityLeftFrame(graph_msf::tryGetParam(node_, "extrinsics.wheelLinearVelocityLeftFrame")); /// Right dynamic_cast(staticTransformsPtr_.get()) - ->setWheelLinearVelocityRightFrame(graph_msf::tryGetParam("extrinsics/wheelLinearVelocityRightFrame", privateNode)); + ->setWheelLinearVelocityRightFrame(graph_msf::tryGetParam(node_, "extrinsics.wheelLinearVelocityRightFrame")); /// VIO Odometry frame dynamic_cast(staticTransformsPtr_.get()) - ->setVioOdometryFrame(graph_msf::tryGetParam("extrinsics/vioOdometryFrame", privateNode)); + ->setVioOdometryFrame(graph_msf::tryGetParam(node_, "extrinsics.vioOdometryFrame")); // Wheel Radius - wheelRadiusMeter_ = graph_msf::tryGetParam("sensor_params/wheelRadius", privateNode); + wheelRadiusMeter_ = graph_msf::tryGetParam(node_, "sensor_params.wheelRadius"); } } // namespace smb_se diff --git a/examples/ros2/smb_estimator_graph_ros2/src/smb_estimator_node.cpp b/examples/ros2/smb_estimator_graph_ros2/src/smb_estimator_node.cpp index 18775d2d..d07e95a3 100644 --- a/examples/ros2/smb_estimator_graph_ros2/src/smb_estimator_node.cpp +++ b/examples/ros2/smb_estimator_graph_ros2/src/smb_estimator_node.cpp @@ -5,32 +5,36 @@ This file is released under the "BSD-3-Clause License". Please see the LICENSE file that has been included as part of this package. */ -// ROS -#include +// ROS 2 +#include // Debugging -#include -#include +// #include // Local packages #include "smb_estimator_graph_ros2/SmbEstimator.h" // Main node entry point int main(int argc, char** argv) { + // Initialize ROS 2 + rclcpp::init(argc, argv); + // Debugging - google::InitGoogleLogging(argv[0]); - FLAGS_alsologtostderr = true; - google::InstallFailureSignalHandler(); - - // ROS related - ros::init(argc, argv, "anymal_estimator_graph"); - std::shared_ptr privateNodePtr = std::make_shared("~"); - /// Do multi-threaded spinner - ros::MultiThreadedSpinner spinner(4); - - // Create Instance - smb_se::SmbEstimator anymalEstimator(privateNodePtr); - spinner.spin(); + // FLAGS_alsologtostderr = true; + + // Create Node + auto privateNodePtr = std::make_shared("smb_estimator_node"); + + // Create Instance of SmbEstimator + auto smbEstimator = std::make_shared(privateNodePtr); + + // Use Multi-Threaded Executor + rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 4); + executor.add_node(privateNodePtr); + executor.spin(); + + // Shutdown ROS 2 + rclcpp::shutdown(); return 0; } \ No newline at end of file From d71d18fb6ef79d4317cb7dce4cd0dcc1fd207173 Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Thu, 22 May 2025 15:15:27 +0000 Subject: [PATCH 09/60] Update CMakeLists.txt to include GTSAM package and clean up dependencies. --- .../smb_estimator_graph_ros2/CMakeLists.txt | 30 ++++++++----------- 1 file changed, 12 insertions(+), 18 deletions(-) diff --git a/examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt b/examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt index 7ce7fc38..d40eab57 100644 --- a/examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt +++ b/examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt @@ -20,10 +20,7 @@ find_package(tf2_ros REQUIRED) find_package(graph_msf_ros2 REQUIRED) find_package(graph_msf REQUIRED) find_package(Glog REQUIRED) - -# Display Eigen version and path -message("Eigen Version: ${EIGEN3_VERSION_STRING}") -message("Eigen Path: ${Eigen3_DIR}") +find_package(GTSAM REQUIRED) # Color if(NOT WIN32) @@ -36,10 +33,8 @@ endif() # Define include directories include_directories( include - ${EIGEN3_INCLUDE_DIR} - ${GLOG_INCLUDE_DIRS} - ${graph_msf_ros2_INCLUDE_DIRS} - ${graph_msf_INCLUDE_DIRS} + SYSTEM ${GTSAM_INCLUDE_DIR} + SYSTEM ${GTSAM_UNSTABLE_INCLUDE_DIR} ) # Library @@ -47,14 +42,6 @@ add_library(${PROJECT_NAME} src/lib/SmbEstimator.cpp src/lib/readParams.cpp src/lib/SmbStaticTransforms.cpp - include/smb_estimator_graph_ros2/constants.h -) - -target_link_libraries(${PROJECT_NAME} - ${gflags_LIBRARIES} - ${GLOG_LIBRARIES} - ${graph_msf_LIBRARIES} - ${graph_msf_ros2_LIBRARIES} ) ament_target_dependencies(${PROJECT_NAME} @@ -66,6 +53,15 @@ ament_target_dependencies(${PROJECT_NAME} tf2_ros graph_msf graph_msf_ros2 + Glog + gflags + GTSAM +) + +target_link_libraries(${PROJECT_NAME} + ${GLOG_LIBRARIES} + ${gflags_LIBRARIES} + gtsam gtsam_unstable ) # Executable @@ -74,8 +70,6 @@ target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME} ${gflags_LIBRARIES} ${GLOG_LIBRARIES} - ${graph_msf_LIBRARIES} - ${graph_msf_ros2_LIBRARIES} ) ament_target_dependencies(${PROJECT_NAME}_node From 8c428cdb8f7b032be35ae438ee3c10a95608b2b1 Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Thu, 22 May 2025 15:19:07 +0000 Subject: [PATCH 10/60] Fix CMakeLists.txt to use project name variable in target_link_libraries for improved maintainability. --- graph_msf/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/graph_msf/CMakeLists.txt b/graph_msf/CMakeLists.txt index c1ec3f59..0e1919a1 100644 --- a/graph_msf/CMakeLists.txt +++ b/graph_msf/CMakeLists.txt @@ -51,7 +51,7 @@ add_library(${PROJECT_NAME} SHARED # Link dependencies explicitly ament_target_dependencies(${PROJECT_NAME} Eigen3 GTSAM GTSAM_UNSTABLE tf2_eigen Python3) -target_link_libraries( graph_msf gtsam gtsam_unstable) +target_link_libraries(${PROJECT_NAME} gtsam gtsam_unstable) # Optional: Clang tooling find_package(cmake_clang_tools QUIET) From 138ccc2086eb167571932ffa18a93683c4a3cb77 Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Thu, 22 May 2025 15:19:23 +0000 Subject: [PATCH 11/60] Refactor SmbStaticTransforms constructor to remove dependency on StaticTransforms and simplify initialization with only the node pointer. --- .../include/smb_estimator_graph_ros2/SmbStaticTransforms.h | 3 +-- .../smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp | 5 ++--- 2 files changed, 3 insertions(+), 5 deletions(-) diff --git a/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h b/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h index c2f52b49..71c508c3 100644 --- a/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h +++ b/examples/ros2/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h @@ -16,8 +16,7 @@ namespace smb_se { class SmbStaticTransforms : public graph_msf::StaticTransformsTf { public: - SmbStaticTransforms(const std::shared_ptr& nodePtr, - const graph_msf::StaticTransforms& staticTransforms); + SmbStaticTransforms(const std::shared_ptr& nodePtr); // Setters void setLioOdometryFrame(const std::string& s) { lidarOdometryFrame_ = s; } diff --git a/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp b/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp index 8ff03fa2..07602e88 100644 --- a/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp +++ b/examples/ros2/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp @@ -19,9 +19,8 @@ Please see the LICENSE file that has been included as part of this package. namespace smb_se { -SmbStaticTransforms::SmbStaticTransforms(const std::shared_ptr& nodePtr, - const graph_msf::StaticTransforms& staticTransforms) - : graph_msf::StaticTransformsTf(nodePtr, staticTransforms) { +SmbStaticTransforms::SmbStaticTransforms(const std::shared_ptr& nodePtr) + : graph_msf::StaticTransformsTf(nodePtr, *this) { REGULAR_COUT << GREEN_START << " Initializing static transforms..." << COLOR_END << std::endl; } From 91e357970cb3799f6ca610aca0995a6473f1d6cc Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Fri, 23 May 2025 09:05:23 +0000 Subject: [PATCH 12/60] Replace XML launch files with Python launch files for smb_estimator_graph and smb_estimator_graph_replay, enhancing maintainability and aligning with ROS 2 conventions. --- .../launch/smb_estimator_graph.launch | 48 ---------- .../launch/smb_estimator_graph.launch.py | 92 +++++++++++++++++++ .../launch/smb_estimator_graph_replay.launch | 22 ----- .../smb_estimator_graph_replay.launch.py | 77 ++++++++++++++++ 4 files changed, 169 insertions(+), 70 deletions(-) delete mode 100644 examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch create mode 100644 examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py delete mode 100644 examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch create mode 100644 examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py diff --git a/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch b/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch deleted file mode 100644 index 01055b43..00000000 --- a/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch +++ /dev/null @@ -1,48 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py b/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py new file mode 100644 index 00000000..bd9455c4 --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py @@ -0,0 +1,92 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + # Package name + pkg_name = 'smb_estimator_graph' + + # Get package share directory + pkg_dir = get_package_share_directory(pkg_name) + + # Declare launch arguments + use_sim_time = LaunchConfiguration('use_sim_time') + imu_topic_name = LaunchConfiguration('imu_topic_name') + lidar_odometry_topic_name = LaunchConfiguration('lidar_odometry_topic_name') + wheel_odometry_topic_name = LaunchConfiguration('wheel_odometry_topic_name') + wheel_velocities_topic_name = LaunchConfiguration('wheel_velocities_topic_name') + vio_odometry_topic_name = LaunchConfiguration('vio_odometry_topic_name') + logging_dir_location = LaunchConfiguration('logging_dir_location') + + # Default config file paths + core_graph_config_param_file = os.path.join(pkg_dir, 'config', 'core', 'core_graph_config.yaml') + core_graph_param_file = os.path.join(pkg_dir, 'config', 'core', 'core_graph_params.yaml') + core_extrinsic_param_file = os.path.join(pkg_dir, 'config', 'core', 'core_extrinsic_params.yaml') + smb_graph_param_file = os.path.join(pkg_dir, 'config', 'smb_specific', 'smb_graph_params.yaml') + smb_extrinsic_param_file = os.path.join(pkg_dir, 'config', 'smb_specific', 'smb_extrinsic_params.yaml') + + return LaunchDescription([ + # Declare launch arguments + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation time' + ), + DeclareLaunchArgument( + 'imu_topic_name', + default_value='/imu', + description='IMU topic name' + ), + DeclareLaunchArgument( + 'lidar_odometry_topic_name', + default_value='/mapping/scan2map_odometry', + description='Lidar odometry topic name' + ), + DeclareLaunchArgument( + 'wheel_odometry_topic_name', + default_value='/control/smb_diff_drive/odom', + description='Wheel odometry topic name' + ), + DeclareLaunchArgument( + 'wheel_velocities_topic_name', + default_value='/control/smb_lowlevel_controller/wheelSpeeds', + description='Wheel velocities topic name' + ), + DeclareLaunchArgument( + 'vio_odometry_topic_name', + default_value='/tracking_camera/odom/sample', + description='VIO odometry topic name' + ), + DeclareLaunchArgument( + 'logging_dir_location', + default_value=os.path.join(pkg_dir, 'logging'), + description='Logging directory location' + ), + + # Node + Node( + package='smb_estimator_graph', + executable='smb_estimator_graph_node', + name='smb_estimator_node', + output='screen', + parameters=[ + {'use_sim_time': use_sim_time}, + {'launch/optimizationResultLoggingPath': logging_dir_location}, + core_graph_config_param_file, + core_graph_param_file, + core_extrinsic_param_file, + smb_graph_param_file, + smb_extrinsic_param_file + ], + remappings=[ + ('/imu_topic', imu_topic_name), + ('/lidar_odometry_topic', lidar_odometry_topic_name), + ('/wheel_odometry_topic', wheel_odometry_topic_name), + ('/wheel_velocities_topic', wheel_velocities_topic_name), + ('/vio_odometry_topic', vio_odometry_topic_name) + ] + ) + ]) \ No newline at end of file diff --git a/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch b/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch deleted file mode 100644 index 049494fb..00000000 --- a/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py b/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py new file mode 100644 index 00000000..d4a357c7 --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py @@ -0,0 +1,77 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + # Package name + pkg_name = 'smb_estimator_graph_ros2' + + # Get package share directory + pkg_dir = get_package_share_directory(pkg_name) + + # Declare launch arguments + use_sim_time = LaunchConfiguration('use_sim_time') + imu_topic_name = LaunchConfiguration('imu_topic_name') + lidar_odometry_topic_name = LaunchConfiguration('lidar_odometry_topic_name') + core_graph_param_file = LaunchConfiguration('core_graph_param_file') + smb_graph_param_file = LaunchConfiguration('smb_graph_param_file') + + # Default config file paths + default_core_graph_param_file = os.path.join(pkg_dir, 'config', 'core', 'core_graph_params.yaml') + default_smb_graph_param_file = os.path.join(pkg_dir, 'config', 'smb_specific', 'smb_graph_params.yaml') + + return LaunchDescription([ + # Declare launch arguments + DeclareLaunchArgument( + 'use_sim_time', + default_value='true', + description='Use simulation time' + ), + DeclareLaunchArgument( + 'imu_topic_name', + default_value='/imu', + description='IMU topic name' + ), + DeclareLaunchArgument( + 'lidar_odometry_topic_name', + default_value='/mapping/scan2map_odometry', + description='Lidar odometry topic name' + ), + DeclareLaunchArgument( + 'core_graph_param_file', + default_value=default_core_graph_param_file, + description='Core graph parameter file' + ), + DeclareLaunchArgument( + 'smb_graph_param_file', + default_value=default_smb_graph_param_file, + description='SMB graph parameter file' + ), + + # Include the main launch file + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + os.path.join(pkg_dir, 'launch', 'smb_estimator_graph.launch.py') + ]), + launch_arguments={ + 'use_sim_time': use_sim_time, + 'imu_topic_name': imu_topic_name, + 'lidar_odometry_topic_name': lidar_odometry_topic_name, + 'core_graph_param_file': core_graph_param_file, + 'smb_graph_param_file': smb_graph_param_file + }.items() + ), + + # RViz Node + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', os.path.join(pkg_dir, 'rviz', 'lidar_estimation.rviz')], + output='screen' + ) + ]) \ No newline at end of file From 2e0899bd21fd037cffa25bd2002102c555160025 Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Fri, 23 May 2025 09:05:43 +0000 Subject: [PATCH 13/60] Refactor remove_map_and_odom_from_bag.py to utilize rosbag2 API for reading and writing bags, improving performance and compatibility with ROS 2 conventions. --- .../scripts/remove_map_and_odom_from_bag.py | 106 ++++++++++++------ 1 file changed, 69 insertions(+), 37 deletions(-) diff --git a/examples/ros2/smb_estimator_graph_ros2/scripts/remove_map_and_odom_from_bag.py b/examples/ros2/smb_estimator_graph_ros2/scripts/remove_map_and_odom_from_bag.py index 92495df9..3477d678 100644 --- a/examples/ros2/smb_estimator_graph_ros2/scripts/remove_map_and_odom_from_bag.py +++ b/examples/ros2/smb_estimator_graph_ros2/scripts/remove_map_and_odom_from_bag.py @@ -2,49 +2,81 @@ # Code taken from: https://gist.github.com/awesomebytes/51470efe54b45045c50263f56d7aec27 -import rosbag import sys +from rclpy.serialization import deserialize_message +from rosidl_runtime_py.utilities import get_message +from rosbag2_py import SequentialReader, StorageOptions, ConverterOptions +from rosbag2_py import SequentialWriter, StorageOptions as WriterStorageOptions +from tf2_msgs.msg import TFMessage +import yaml def filter_topics(in_bag, out_bag, frames_we_want): - with rosbag.Bag(out_bag, 'w') as outbag: - print("Writing to " + out_bag) - print("Reading from " + in_bag) - for topic, msg, t in rosbag.Bag(in_bag).read_messages(): - if topic == "/tf" and msg.transforms: - transforms_to_keep = [] - for i in range(len(msg.transforms)): - # if its one of the frames we want we keep it - if msg.transforms[i].header.frame_id in frames_we_want and msg.transforms[i].child_frame_id in frames_we_want: - transforms_to_keep.append(msg.transforms[i]) - #print("Keeping: " + str(msg.transforms[i])) - # else: - # print("Discarding: " + str(msg.transforms[i])) - - msg.transforms = transforms_to_keep - outbag.write(topic, msg, t) - elif topic != '/tf': - outbag.write(topic, msg, t) + # Setup reader + storage_options = StorageOptions(uri=in_bag, storage_id='sqlite3') + converter_options = ConverterOptions('', '') + reader = SequentialReader() + reader.open(storage_options, converter_options) + + # Setup writer + writer_storage_options = WriterStorageOptions(uri=out_bag, storage_id='sqlite3') + writer = SequentialWriter() + writer.open(writer_storage_options, converter_options) + + # Get topic types + topic_types = reader.get_all_topics_and_types() + type_map = {topic_info.name: topic_info.type for topic_info in topic_types} + + print("Writing to " + out_bag) + print("Reading from " + in_bag) + + while reader.has_next(): + topic_name, data, timestamp = reader.read_next() + msg_type = get_message(type_map[topic_name]) + msg = deserialize_message(data, msg_type) + + if topic_name == "/tf" and isinstance(msg, TFMessage): + transforms_to_keep = [] + for transform in msg.transforms: + if transform.header.frame_id in frames_we_want and transform.child_frame_id in frames_we_want: + transforms_to_keep.append(transform) + msg.transforms = transforms_to_keep + writer.write(topic_name, data, timestamp) + elif topic_name != '/tf': + writer.write(topic_name, data, timestamp) def filter_topics_2(in_bag, out_bag, frames_we_dont_want): - with rosbag.Bag(out_bag, 'w') as outbag: - print("Writing to " + out_bag) - print("Reading from " + in_bag) - for topic, msg, t in rosbag.Bag(in_bag).read_messages(): - if topic == "/tf" and msg.transforms: - transforms_to_keep = [] - for i in range(len(msg.transforms)): - # if its one of the frames we want we keep it - if msg.transforms[i].header.frame_id not in frames_we_dont_want and msg.transforms[i].child_frame_id not in frames_we_dont_want: - transforms_to_keep.append(msg.transforms[i]) - #print("Keeping: " + str(msg.transforms[i])) - # else: - # print("Discarding: " + str(msg.transforms[i])) - - msg.transforms = transforms_to_keep - outbag.write(topic, msg, t) - elif topic != '/tf': - outbag.write(topic, msg, t) + # Setup reader + storage_options = StorageOptions(uri=in_bag, storage_id='sqlite3') + converter_options = ConverterOptions('', '') + reader = SequentialReader() + reader.open(storage_options, converter_options) + + # Setup writer + writer_storage_options = WriterStorageOptions(uri=out_bag, storage_id='sqlite3') + writer = SequentialWriter() + writer.open(writer_storage_options, converter_options) + + # Get topic types + topic_types = reader.get_all_topics_and_types() + type_map = {topic_info.name: topic_info.type for topic_info in topic_types} + + print("Writing to " + out_bag) + print("Reading from " + in_bag) + + while reader.has_next(): + topic_name, data, timestamp = reader.read_next() + msg_type = get_message(type_map[topic_name]) + msg = deserialize_message(data, msg_type) + if topic_name == "/tf" and isinstance(msg, TFMessage): + transforms_to_keep = [] + for transform in msg.transforms: + if transform.header.frame_id not in frames_we_dont_want and transform.child_frame_id not in frames_we_dont_want: + transforms_to_keep.append(transform) + msg.transforms = transforms_to_keep + writer.write(topic_name, data, timestamp) + elif topic_name != '/tf': + writer.write(topic_name, data, timestamp) if __name__ == '__main__': print("Starting") From c9690b0b262b1fb0cd214568df090d6c1e9f7e59 Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Fri, 23 May 2025 09:17:54 +0000 Subject: [PATCH 14/60] Enhance error handling and logging in remove_tf_from_bag.py --- .../bag_filter/remove_tf_from_bag.py | 100 +++++++++++------- 1 file changed, 60 insertions(+), 40 deletions(-) diff --git a/ros2/graph_msf_ros2/graph_msf_ros2_py/bag_filter/remove_tf_from_bag.py b/ros2/graph_msf_ros2/graph_msf_ros2_py/bag_filter/remove_tf_from_bag.py index 04114575..660082f1 100644 --- a/ros2/graph_msf_ros2/graph_msf_ros2_py/bag_filter/remove_tf_from_bag.py +++ b/ros2/graph_msf_ros2/graph_msf_ros2_py/bag_filter/remove_tf_from_bag.py @@ -6,51 +6,71 @@ import rosbag2_py from rclpy.serialization import deserialize_message, serialize_message from rosidl_runtime_py.utilities import get_message +import logging +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger(__name__) def filter_rosbag(input_bag_path, output_bag_path): - # Initialize the rosbag2 reader - reader = rosbag2_py.SequentialReader() - storage_options = rosbag2_py.StorageOptions( - uri=input_bag_path, storage_id="sqlite3" - ) - converter_options = rosbag2_py.ConverterOptions( - input_serialization_format="cdr", output_serialization_format="cdr" - ) - reader.open(storage_options, converter_options) - - # Get all topics and types from the input bag - topic_types = reader.get_all_topics_and_types() - type_map = {topic.name: topic.type for topic in topic_types} - - # Initialize the rosbag2 writer - writer = rosbag2_py.SequentialWriter() - writer_storage_options = rosbag2_py.StorageOptions( - uri=output_bag_path, storage_id="sqlite3" - ) - writer.open(writer_storage_options, converter_options) - - # Create topics in the output bag based on the input bag's topics - for topic in topic_types: - writer.create_topic(topic) - - # Loop through each topic and message in the input rosbag - while reader.has_next(): - topic, data, timestamp = reader.read_next() - - # Skip the /tf topic - if topic == "/tf": - continue - - # Deserialize and re-serialize the message for writing - msg_type = get_message(type_map[topic]) - msg = deserialize_message(data, msg_type) - writer.write(topic, serialize_message(msg), timestamp) + try: + # Initialize the rosbag2 reader + reader = rosbag2_py.SequentialReader() + storage_options = rosbag2_py.StorageOptions( + uri=input_bag_path, storage_id="sqlite3" + ) + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format="cdr", output_serialization_format="cdr" + ) + reader.open(storage_options, converter_options) + # Get all topics and types from the input bag + topic_types = reader.get_all_topics_and_types() + type_map = {topic.name: topic.type for topic in topic_types} + + # Initialize the rosbag2 writer + writer = rosbag2_py.SequentialWriter() + writer_storage_options = rosbag2_py.StorageOptions( + uri=output_bag_path, storage_id="sqlite3" + ) + writer.open(writer_storage_options, converter_options) + + # Create topics in the output bag based on the input bag's topics + for topic in topic_types: + writer.create_topic(topic) + + # Loop through each topic and message in the input rosbag + while reader.has_next(): + try: + topic, data, timestamp = reader.read_next() + + # Skip the /tf topic + if topic == "/tf": + continue + + # Deserialize and re-serialize the message for writing + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + serialized_msg = serialize_message(msg) + writer.write(topic, serialized_msg, timestamp) + except Exception as e: + logger.warning(f"Error processing message on topic {topic}: {str(e)}") + continue + + except Exception as e: + logger.error(f"Error processing bag: {str(e)}") + raise if __name__ == "__main__": - print("Starting") + if len(sys.argv) != 3: + logger.error("Usage: python3 remove_tf_from_bag.py input_bag output_bag") + sys.exit(1) + + logger.info("Starting") input_bag = sys.argv[1] output_bag = sys.argv[2] - filter_rosbag(input_bag_path=input_bag, output_bag_path=output_bag) - print("Done") + try: + filter_rosbag(input_bag_path=input_bag, output_bag_path=output_bag) + logger.info("Done") + except Exception as e: + logger.error(f"Failed to process bag: {str(e)}") + sys.exit(1) From 6b464ff14c46a5ea87b5fcaa0e4da66f771a1e6b Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Fri, 23 May 2025 09:22:27 +0000 Subject: [PATCH 15/60] Enhance plotting functionality and logging in plot_latest_quantitites_in_folder.py, adding support for covariance data and improving error handling. --- .../plot_latest_quantitites_in_folder.py | 321 ++++++++++-------- 1 file changed, 181 insertions(+), 140 deletions(-) diff --git a/ros2/graph_msf_ros2/graph_msf_ros2_py/plotting/plot_latest_quantitites_in_folder.py b/ros2/graph_msf_ros2/graph_msf_ros2_py/plotting/plot_latest_quantitites_in_folder.py index fa4fd243..78e2901a 100644 --- a/ros2/graph_msf_ros2/graph_msf_ros2_py/plotting/plot_latest_quantitites_in_folder.py +++ b/ros2/graph_msf_ros2/graph_msf_ros2_py/plotting/plot_latest_quantitites_in_folder.py @@ -5,6 +5,11 @@ import matplotlib.pyplot as plt import numpy as np import sys +from ament_index_python.packages import get_package_share_directory +import logging + +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger(__name__) # Path to the folder containing the files HOME_DIR = str(os.path.expanduser("~")) @@ -20,158 +25,194 @@ import utils.get_latest_time_string_in_folder as get_latest_time_string_in_folder -def plot_quantities_in_file( - file_path: str, type: str, dir_path: str, latest_file_string: str -): - # Data - fields = [] - if "bias" in type: - fields = ["t", "a_x", "a_y", "a_z", "w_x", "w_y", "w_z"] - elif "transform" in type or "pose" in type: - fields = ["t", "x", "y", "z", "qw", "qx", "qy", "qz", "roll", "pitch", "yaw"] - elif "velocity" in type: - fields = ["t", "v_x", "v_y", "v_z"] - - # Read - data = [] - first_line = True - with open(file_path, "r") as file: - for line in file: - # Header file - if first_line: - first_line = False - continue - # Assuming your file format is: timestamp,x,y,z,qw,qx,qy,qz - parts = line.strip().split(",") - - # Write to data - if len(parts) < len(fields): - continue # Skip malformed lines - data.append(np.asarray(parts)) - - # Process Data - data = np.array(data, dtype=np.float64) - # Subtract first timestamp - data[:, 0] = data[:, 0] - data[0, 0] - print(f"Data shape: {data.shape}") - print("First 5 rows of data: ", data[:5, :]) - # Sorty by time - # indices = np.argsort(data[:, 0]) - # data = data[indices, :] - # print(indices) - # Plot - plt.figure() - for i, field in enumerate(fields): - # Do not plot time field - if i == 0 or field == "qw" or field == "qx" or field == "qy" or field == "qz": - continue - else: - print(f"Plotting field: {field}") - # Create figure - plt.plot(data[:, 0], data[:, i], label=field) - - # Create plot - if ("transform" in type or "pose" in type) and field == "z": - plt.xlabel("Time [s]") - plt.ylabel("Position [m]") - plt.title(f"{type} Position Over Time") - plt.legend() - plt.grid() - # Save the plot - plot_path = os.path.join(dir_path, f"{latest_file_string}_plot_{type}_position.png") - plt.savefig(plot_path) +def plot_quantities_in_file(file_path: str, type: str, dir_path: str, latest_file_string: str): + try: + # Data + fields = [] + # Bias + if "bias" in type: + fields = ["t", "a_x", "a_y", "a_z", "w_x", "w_y", "w_z"] + # Covariance + elif "covariance" in type: + fields = ["t", + "cov_00", "cov_01", "cov_02", "cov_03", "cov_04", "cov_05", + "cov_10", "cov_11", "cov_12", "cov_13", "cov_14", "cov_15", + "cov_20", "cov_21", "cov_22", "cov_23", "cov_24", "cov_25", + "cov_30", "cov_31", "cov_32", "cov_33", "cov_34", "cov_35", + "cov_40", "cov_41", "cov_42", "cov_43", "cov_44", "cov_45", + "cov_50", "cov_51", "cov_52", "cov_53", "cov_54", "cov_55"] + elif "transform" in type or "pose" in type: + fields = ["t", "x", "y", "z", "qw", "qx", "qy", "qz", "roll", "pitch", "yaw"] + elif "velocity" in type: + fields = ["t", "v_x", "v_y", "v_z"] + + # Read data + data = [] + first_line = True + with open(file_path, "r") as file: + type = type.split(".")[0] # Remove everything after the dot + for line in file: + if first_line: + first_line = False + continue + parts = line.strip().split(",") + if len(parts) < len(fields): + logger.warning(f"Skipping malformed line: {line}") + continue + data.append(np.asarray(parts)) + + if not data: + logger.error(f"No valid data found in {file_path}") + return + + # Process Data + data = np.array(data, dtype=np.float64) + data[:, 0] = data[:, 0] - data[0, 0] # Subtract first timestamp + logger.info(f"Data shape: {data.shape}") + logger.info(f"Fields: {fields}") + + # Plot + plt.figure() + if "covariance" not in type: + for i, field in enumerate(fields): + if i == 0 or field in ["qw", "qx", "qy", "qz"]: + continue + + logger.info(f"Plotting field: {field}. Max: {np.max(data[:, i])}. Min: {np.min(data[:, i])}") + plt.plot(data[:, 0], data[:, i], label=field) + + # Create plot when entering last field for each type + if "bias" in type and field == "a_z": + plt.xlabel("Time [s]") + plt.ylabel("Acceleration [m/s^2]") + plt.title(f"{type} Acceleration Over Time") + plt.legend() + plt.grid() + plot_path = os.path.join(dir_path, f"{latest_file_string}{type}_acceleration.png") + plt.savefig(plot_path) + plt.figure() + elif "bias" in type and field == "w_z": + plt.xlabel("Time [s]") + plt.ylabel("Angular Velocity [rad/s]") + plt.title(f"{type} Angular Velocity Over Time") + plt.legend() + plt.grid() + plot_path = os.path.join(dir_path, f"{latest_file_string}{type}_angular_velocity.png") + plt.savefig(plot_path) + elif ("transform" in type or "pose" in type) and field == "z": + plt.xlabel("Time [s]") + plt.ylabel("Position [m]") + plt.title(f"{type} Position Over Time") + plt.legend() + plt.grid() + plot_path = os.path.join(dir_path, f"{latest_file_string}{type}_position.png") + plt.savefig(plot_path) + plt.figure() + elif ("transform" in type or "pose" in type) and field == "yaw": + plt.xlabel("Time [s]") + plt.ylabel("Orientation [rad]") + plt.title(f"{type} Orientation Over Time") + plt.legend() + plt.grid() + plot_path = os.path.join(dir_path, f"{latest_file_string}{type}_orientation.png") + plt.savefig(plot_path) + plt.figure() + elif "velocity" in type and field == "v_z": + plt.xlabel("Time [s]") + plt.ylabel("Velocity [m/s]") + plt.title(f"{type} Velocity Over Time") + plt.legend() + plt.grid() + plot_path = os.path.join(dir_path, f"{latest_file_string}{type}.png") + plt.savefig(plot_path) + plt.figure() + + logger.info(f"Plot saved to: {plot_path}") + + # Covariance and x-y plots + if "covariance" in type: plt.figure() - elif ("transform" in type or "pose" in type) and field == "yaw": + plt.plot(data[:, 0], data[:, 1], label="cov_xx") + plt.plot(data[:, 0], data[:, 8], label="cov_yy") + plt.plot(data[:, 0], data[:, 15], label="cov_zz") plt.xlabel("Time [s]") - plt.ylabel("Orientation [rad]") - plt.title(f"{type} Orientation Over Time") + plt.ylabel("Covariance [m]") + plt.title(f"{type} Covariance Over Time") plt.legend() plt.grid() - # Save the plot - plot_path = os.path.join(dir_path, f"{latest_file_string}_plot_{type}_orientation.png") + plot_path = os.path.join(dir_path, f"{latest_file_string}{type}.png") plt.savefig(plot_path) + logger.info(f"Covariance plot saved to: {plot_path}") + elif "transform" in type or "pose" in type: plt.figure() - elif "velocity" in type and field == "v_z": - plt.xlabel("Time [s]") - plt.ylabel("Velocity [m/s]") - plt.title(f"{type} Velocity Over Time") - plt.legend() - plt.grid() - # Save the plot - plot_path = os.path.join(dir_path, f"{latest_file_string}_plot_{type}_velocity.png") - plt.savefig(plot_path) - elif "bias" in type and field == "a_z": - plt.xlabel("Time [s]") - plt.ylabel("Acceleration [m/s^2]") - plt.title(f"{type} Acceleration Over Time") + plt.plot(data[:, 1], data[:, 2], label="x-y") + plt.xlabel("x [m]") + plt.ylabel("y [m]") + plt.title(f"{type} x-y Position Over Time") + plt.axis("equal") plt.legend() plt.grid() - # Save the plot - plot_path = os.path.join(dir_path, f"{latest_file_string}_plot_{type}_acceleration.png") + plot_path = os.path.join(dir_path, f"{latest_file_string}{type}_x_y_position.png") plt.savefig(plot_path) - plt.figure() - elif "bias" in type and field == "w_z": - plt.xlabel("Time [s]") - plt.ylabel("Angular Velocity [rad/s]") - plt.title(f"{type} Angular Velocity Over Time") - plt.legend() - plt.grid() - # Save the plot - plot_path = os.path.join(dir_path, f"{latest_file_string}_plot_{type}_angular_velocity.png") - plt.savefig(plot_path) - else: # Continue plotting - continue - - print(f"Plot saved to: {plot_path}") - - # Now get the x, y plot - if "transform" in type or "pose" in type: - plt.plot(data[:, 1], data[:, 2], label="x-y") - plt.xlabel("x [m]") - plt.ylabel("y [m]") - plt.title(f"{type} x-y Position Over Time") - plt.legend() - plt.grid() - # Save the plot - plot_path = os.path.join(dir_path, f"{latest_file_string}_plot_{type}_x_y_position.png") - plt.savefig(plot_path) - print(f"Plot saved to: {plot_path}") - + logger.info(f"X-Y plot saved to: {plot_path}") + except Exception as e: + logger.error(f"Error plotting {type}: {str(e)}") + raise def main(): - # We want to only get the latest files - latest_file_string = ( - get_latest_time_string_in_folder.get_latest_time_string_in_folder(dir_path) - ) - print("Latest file string: ", latest_file_string) - # Plot following files: - file_types = [ - "B_imu_bias", - "R_6D_transform", - "v_state_3D_velocity", - "X_state_6D_pose", - ] - # Get all files in the folder that contain the latest file string - files = os.listdir(dir_path) - files = [f for f in files if (latest_file_string in f and f.endswith(".csv"))] - print(f"Files containing {latest_file_string}: {files}") - - # Iterate through files and check if they contain the file types - for file in files: - for file_type in file_types: - if file_type in file: - print(f"Plotting {file_type} from file: {file}") - # Plot the quantities in the file - plot_quantities_in_file( - os.path.join(dir_path, file), - file_type, - dir_path, - latest_file_string, - ) - + try: + if len(sys.argv) != 2: + logger.error("Usage: python3 plot_latest_quantitites_in_folder.py ") + sys.exit(1) + + package_name = sys.argv[1] + logger.info(f"Package name: {package_name}") + + # Get package directory + try: + package_dir = get_package_share_directory(package_name) + dir_path = os.path.join(package_dir, "logging") + logger.info(f"Directory path: {dir_path}") + except Exception as e: + logger.error(f"Failed to find package {package_name}: {str(e)}") + sys.exit(1) + + # Get latest directory + latest_dir_string = get_latest_time_string_in_folder.get_latest_time_string_in_folder(dir_path) + logger.info(f"Latest directory: {latest_dir_string}") + + # File types to plot + file_types = [ + "B_imu_bias", + "R_6D_transform", + "v_state_3D_velocity", + "X_state_6D_pose", + "X_state_6D_pose_covariance", + ] + + # Get files + files = os.listdir(os.path.join(dir_path, latest_dir_string)) + files = [f for f in files if f.endswith(".csv")] + logger.info(f"Files in {latest_dir_string}: {files}") + + # Plot files + for file in files: + for file_type in file_types: + if file_type in file: + logger.info(f"Plotting {file_type} from {file}") + plot_quantities_in_file( + file_path=os.path.join(dir_path, latest_dir_string, file), + type=file, + dir_path=os.path.join(dir_path, latest_dir_string), + latest_file_string="", + ) + + except Exception as e: + logger.error(f"Error in main: {str(e)}") + sys.exit(1) if __name__ == "__main__": - print("Starting") + logger.info("Starting") main() - print("Done") + logger.info("Done") From 54d0cafcba71c3df50991a243ac50f90dd8675ac Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Fri, 23 May 2025 09:48:19 +0000 Subject: [PATCH 16/60] Add new parameters for marginals computation and alignment in readParams function --- ros2/graph_msf_ros2/src/lib/readParams.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/ros2/graph_msf_ros2/src/lib/readParams.cpp b/ros2/graph_msf_ros2/src/lib/readParams.cpp index a0232773..1891c99b 100644 --- a/ros2/graph_msf_ros2/src/lib/readParams.cpp +++ b/ros2/graph_msf_ros2/src/lib/readParams.cpp @@ -64,6 +64,10 @@ void GraphMsfRos2::readParams() { tryGetParam(node_, "graph_params.slowBatchSmootherUseCholeskyFactorization"); graphConfigPtr_->usingBiasForPreIntegrationFlag_ = tryGetParam(node_, "graph_params.usingBiasForPreIntegration"); + graphConfigPtr_->useWindowForMarginalsComputationFlag_ = + tryGetParam(node_, "graph_params.useWindowForMarginalsComputation"); + graphConfigPtr_->windowSizeSecondsForMarginalsComputation_ = + tryGetParam(node_, "graph_params.windowSizeSecondsForMarginalsComputation"); graphConfigPtr_->optimizeReferenceFramePosesWrtWorldFlag_ = tryGetParam(node_, "graph_params.optimizeReferenceFramePosesWrtWorld"); graphConfigPtr_->optimizeExtrinsicSensorToSensorCorrectedOffsetFlag_ = @@ -72,6 +76,10 @@ void GraphMsfRos2::readParams() { // Alignment Parameters graphConfigPtr_->referenceFramePosesResetThreshold_ = tryGetParam(node_, "graph_params.referenceFramePosesResetThreshold"); + graphConfigPtr_->centerMeasurementsAtKeyframePositionBeforeAlignmentFlag_ = + tryGetParam(node_, "graph_params.centerMeasurementsAtKeyframePositionBeforeAlignment"); + graphConfigPtr_->createReferenceAlignmentKeyframeEveryNSeconds_ = + tryGetParam(node_, "graph_params.createReferenceAlignmentKeyframeEveryNSeconds"); // graphConfigPtr_->centerReferenceFramesAtRobotPositionBeforeAlignment_ = // tryGetParam(node_, "graph_params.centerReferenceFramesAtRobotPositionBeforeAlignment"); @@ -128,6 +136,9 @@ void GraphMsfRos2::readParams() { // Common Parameters graphConfigPtr_->verboseLevel_ = tryGetParam(node_, "common_params.verbosity"); graphConfigPtr_->odomNotJumpAtStartFlag_ = tryGetParam(node_, "common_params.odomNotJumpAtStart"); + graphConfigPtr_->logRealTimeStateToMemoryFlag_ = tryGetParam(node_, "common_params.logRealTimeStateToMemory"); + graphConfigPtr_->logLatencyAndUpdateDurationToMemoryFlag_ = + tryGetParam(node_, "common_params.logLatencyAndUpdateDurationToMemory"); // Set frames staticTransformsPtr_->setWorldFrame(tryGetParam(node_, "extrinsics.worldFrame")); From a30c404d5b991acb32cfbc0d3866e28eb180f84f Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Fri, 23 May 2025 09:48:47 +0000 Subject: [PATCH 17/60] Refactor setup.py to use setuptools, adding package metadata and entry points for console scripts. Introduce new resource file for graph_msf_ros2. --- ros2/graph_msf_ros2/resource/graph_msf_ros2 | 1 + ros2/graph_msf_ros2/setup.py | 34 +++++++++++++++++---- 2 files changed, 29 insertions(+), 6 deletions(-) create mode 100644 ros2/graph_msf_ros2/resource/graph_msf_ros2 diff --git a/ros2/graph_msf_ros2/resource/graph_msf_ros2 b/ros2/graph_msf_ros2/resource/graph_msf_ros2 new file mode 100644 index 00000000..0519ecba --- /dev/null +++ b/ros2/graph_msf_ros2/resource/graph_msf_ros2 @@ -0,0 +1 @@ + \ No newline at end of file diff --git a/ros2/graph_msf_ros2/setup.py b/ros2/graph_msf_ros2/setup.py index f0bc660f..9272c5e6 100644 --- a/ros2/graph_msf_ros2/setup.py +++ b/ros2/graph_msf_ros2/setup.py @@ -1,8 +1,30 @@ -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +from setuptools import setup +from setuptools import find_packages -setup_args = generate_distutils_setup( - package_dir={"": "python"}, -) +package_name = 'graph_msf_ros2' -setup(**setup_args) \ No newline at end of file +setup( + name=package_name, + version='0.0.0', + packages=find_packages(where='graph_msf_ros2_py'), + package_dir={'': 'graph_msf_ros2_py'}, + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Julian Nubert', + maintainer_email='nubertj@ethz.ch', + description='State estimation based on factor graphs, utilizing GTSAM functionality', + license='BSD', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'manual_pose_files_to_tf_and_odom_bag = graph_msf_ros2_py.replay.manual_pose_files_to_tf_and_odom_bag:main', + 'plot_latest_quantitites_in_folder = graph_msf_ros2_py.plotting.plot_latest_quantitites_in_folder:main', + 'remove_tf_from_bag = graph_msf_ros2_py.bag_filter.remove_tf_from_bag:main', + ], + }, +) \ No newline at end of file From 432352b76a31df354ae5e8c3a58342758088a4ce Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Fri, 23 May 2025 09:49:26 +0000 Subject: [PATCH 18/60] Refactor manual_pose_files_to_tf_and_odom_bag.py to improve file processing and logging. --- .../manual_pose_files_to_tf_and_odom_bag.py | 99 ++++++++++++++----- 1 file changed, 76 insertions(+), 23 deletions(-) diff --git a/ros2/graph_msf_ros2/graph_msf_ros2_py/replay/manual_pose_files_to_tf_and_odom_bag.py b/ros2/graph_msf_ros2/graph_msf_ros2_py/replay/manual_pose_files_to_tf_and_odom_bag.py index 0497d71a..824d5a21 100644 --- a/ros2/graph_msf_ros2/graph_msf_ros2_py/replay/manual_pose_files_to_tf_and_odom_bag.py +++ b/ros2/graph_msf_ros2/graph_msf_ros2_py/replay/manual_pose_files_to_tf_and_odom_bag.py @@ -1,37 +1,90 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 -# Imports import os.path +import sys +import logging from pathlib import Path +from ament_index_python.packages import get_package_share_directory -HOME_DIR = str(Path.home()) +# Add the parent directory to sys.path +sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) # Workspace from graph_msf_ros2_py.replay.transform_helpers import write_bag -from graph_msf_ros2_py.utils.get_latest_time_string_in_folder import ( - get_latest_time_string_in_folder, -) - -# Input and output files -dir_path = os.path.join( - HOME_DIR, - "workspaces/rsl_workspaces/graph_msf_ws/src/graph_msf_dev/examples/anymal_estimator_graph/logging", -) -latest_time_string = get_latest_time_string_in_folder(folder_path=dir_path) -input_file_path = os.path.join( - dir_path, latest_time_string + "_optimized_X_state_transform.csv" -) -output_bag_path = os.path.join( - dir_path, latest_time_string + "_bag_X_state_transform.bag" -) -fixed_frame_id = "offline_world_graph_msf" -child_frame_id = "imu_link" +from graph_msf_ros2_py.utils.get_latest_time_string_in_folder import get_latest_time_string_in_folder +logging.basicConfig(level=logging.INFO) +logger = logging.getLogger(__name__) + +def process_file(dir_path: str, csv_file_name: str, fixed_frame_id: str, child_frame_id: str): + try: + latest_time_string = get_latest_time_string_in_folder(folder_path=dir_path) + input_file_path = os.path.join(dir_path, latest_time_string, f"{csv_file_name}.csv") + output_bag_path = os.path.join(dir_path, latest_time_string, f"bag_{csv_file_name}.bag") + + logger.info(f"Processing file: {input_file_path}") + logger.info(f"Output bag: {output_bag_path}") + + write_bag( + input_file_path=input_file_path, + output_bag_path=output_bag_path, + fixed_frame_id=fixed_frame_id, + child_frame_id=child_frame_id + ) + logger.info(f"Successfully processed {csv_file_name}") + except Exception as e: + logger.error(f"Error processing {csv_file_name}: {str(e)}") + raise def main(): - # Go through the input file and write the data to the output bag - write_bag(input_file_path, output_bag_path, fixed_frame_id, child_frame_id) + try: + # Get command line arguments + if len(sys.argv) != 3: + logger.error("Usage: python3 manual_pose_files_to_tf_and_odom_bag.py ") + sys.exit(1) + + package_name = sys.argv[1] + imu_frame_name = sys.argv[2] + + # Validate IMU frame name + valid_frames = ["cpt7_imu", "imu_link"] + if imu_frame_name not in valid_frames: + logger.error(f"Invalid IMU frame name. Must be one of: {valid_frames}") + sys.exit(1) + + logger.info(f"Package name: {package_name}") + logger.info(f"IMU frame name: {imu_frame_name}") + + # Get package directory + try: + package_dir = get_package_share_directory(package_name) + dir_path = os.path.join(package_dir, "logging") + logger.info(f"Directory path: {dir_path}") + except Exception as e: + logger.error(f"Failed to find package {package_name}: {str(e)}") + sys.exit(1) + + # Process files + files_to_process = [ + "X_state_6D_pose", + "rt_world_x_state_6D_pose", + "optimized_X_state_transform" # Additional file from ROS2 version + ] + + fixed_frame_id = "gt_world" # or "offline_world_graph_msf" depending on your needs + + for file_name in files_to_process: + try: + process_file(dir_path, file_name, fixed_frame_id, imu_frame_name) + except Exception as e: + logger.warning(f"Skipping {file_name} due to error: {str(e)}") + continue + except Exception as e: + logger.error(f"Error in main: {str(e)}") + sys.exit(1) if __name__ == "__main__": + logger.info("Starting") main() + logger.info("Done") From 8534deafae95e298fc7ab277d7694c43e0d8b90a Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Fri, 23 May 2025 11:01:36 +0000 Subject: [PATCH 19/60] Update package name in smb_estimator_graph.launch.py to smb_estimator_graph_ros2 --- .../launch/smb_estimator_graph.launch.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py b/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py index bd9455c4..cd3c89b4 100644 --- a/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py +++ b/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py @@ -7,7 +7,7 @@ def generate_launch_description(): # Package name - pkg_name = 'smb_estimator_graph' + pkg_name = 'smb_estimator_graph_ros2' # Get package share directory pkg_dir = get_package_share_directory(pkg_name) @@ -68,8 +68,8 @@ def generate_launch_description(): # Node Node( - package='smb_estimator_graph', - executable='smb_estimator_graph_node', + package='smb_estimator_graph_ros2', + executable='smb_estimator_graph_ros2_node', name='smb_estimator_node', output='screen', parameters=[ From 4b78b8d961e4f84641e8dcb3cbf9bc31f7419867 Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Fri, 23 May 2025 11:41:39 +0000 Subject: [PATCH 20/60] Enhance CMakeLists.txt by adding GTSAM_UNSTABLE package --- .../smb_estimator_graph_ros2/CMakeLists.txt | 29 ++++++++++++++----- 1 file changed, 22 insertions(+), 7 deletions(-) diff --git a/examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt b/examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt index d40eab57..2a6ae5f3 100644 --- a/examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt +++ b/examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt @@ -21,6 +21,11 @@ find_package(graph_msf_ros2 REQUIRED) find_package(graph_msf REQUIRED) find_package(Glog REQUIRED) find_package(GTSAM REQUIRED) +find_package(GTSAM_UNSTABLE REQUIRED) + +# Display Eigen version and path +message("Eigen Version: ${EIGEN3_VERSION_STRING}") +message("Eigen Path: ${Eigen3_DIR}") # Color if(NOT WIN32) @@ -35,6 +40,9 @@ include_directories( include SYSTEM ${GTSAM_INCLUDE_DIR} SYSTEM ${GTSAM_UNSTABLE_INCLUDE_DIR} + ${EIGEN3_INCLUDE_DIR} + ${graph_msf_ros2_INCLUDE_DIRS} + ${graph_msf_INCLUDE_DIRS} ) # Library @@ -44,6 +52,15 @@ add_library(${PROJECT_NAME} src/lib/SmbStaticTransforms.cpp ) +target_link_libraries(${PROJECT_NAME} + gtsam + gtsam_unstable + ${graph_msf_LIBRARIES} + ${graph_msf_ros2_LIBRARIES} + ${GLOG_LIBRARIES} + ${gflags_LIBRARIES} +) + ament_target_dependencies(${PROJECT_NAME} rclcpp std_msgs @@ -58,18 +75,16 @@ ament_target_dependencies(${PROJECT_NAME} GTSAM ) -target_link_libraries(${PROJECT_NAME} - ${GLOG_LIBRARIES} - ${gflags_LIBRARIES} - gtsam gtsam_unstable -) - # Executable add_executable(${PROJECT_NAME}_node src/smb_estimator_node.cpp) target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME} - ${gflags_LIBRARIES} + gtsam + gtsam_unstable + ${graph_msf_LIBRARIES} + ${graph_msf_ros2_LIBRARIES} ${GLOG_LIBRARIES} + ${gflags_LIBRARIES} ) ament_target_dependencies(${PROJECT_NAME}_node From b2f85c831ff5fd732fbe39e560274a27bcd14414 Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Fri, 23 May 2025 11:55:33 +0000 Subject: [PATCH 21/60] Add export section to package.xml for launch file configuration --- examples/ros2/smb_estimator_graph_ros2/package.xml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/examples/ros2/smb_estimator_graph_ros2/package.xml b/examples/ros2/smb_estimator_graph_ros2/package.xml index 949603dd..0a2434b5 100644 --- a/examples/ros2/smb_estimator_graph_ros2/package.xml +++ b/examples/ros2/smb_estimator_graph_ros2/package.xml @@ -29,4 +29,9 @@ ament_cmake_gtest rosbag2 + + + + ament_cmake + From 3a01e903043bb30d5e253fcb1dd1ab1c2e2e14c3 Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Fri, 23 May 2025 11:55:45 +0000 Subject: [PATCH 22/60] Add export for include directories in CMakeLists.txt to improve package integration --- examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt b/examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt index 2a6ae5f3..1361a335 100644 --- a/examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt +++ b/examples/ros2/smb_estimator_graph_ros2/CMakeLists.txt @@ -129,5 +129,8 @@ install(DIRECTORY launch rviz config DESTINATION share/${PROJECT_NAME} ) +# Export include directories +ament_export_include_directories(include) + # Finalize ament package ament_package() From 79a1f81577155b1732b5bbb68956058dc82fe55a Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Fri, 23 May 2025 12:04:52 +0000 Subject: [PATCH 23/60] Update parameter declarations in GraphMsfRos2.cpp to include new settings for Cholesky factorization and marginals computation, while commenting out the center reference frames parameter. --- ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp index 34b435d7..a5bf9191 100644 --- a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp +++ b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp @@ -49,12 +49,16 @@ void GraphMsfRos2::setup(std::shared_ptr staticTransformsPtr) node_->declare_parameter("graph_params.additionalOptimizationIterations", 0); node_->declare_parameter("graph_params.findUnusedFactorSlots", false); node_->declare_parameter("graph_params.enableDetailedResults", false); - node_->declare_parameter("graph_params.usingCholeskyFactorization", false); + node_->declare_parameter("graph_params.realTimeSmootherUseCholeskyFactorization", false); + node_->declare_parameter("graph_params.slowBatchSmootherUseCholeskyFactorization", false); node_->declare_parameter("graph_params.usingBiasForPreIntegration", false); node_->declare_parameter("graph_params.optimizeReferenceFramePosesWrtWorld", false); node_->declare_parameter("graph_params.optimizeExtrinsicSensorToSensorCorrectedOffset", false); node_->declare_parameter("graph_params.referenceFramePosesResetThreshold", 0.0); - node_->declare_parameter("graph_params.centerReferenceFramesAtRobotPositionBeforeAlignment", false); + // node_->declare_parameter("graph_params.centerReferenceFramesAtRobotPositionBeforeAlignment", false); + node_->declare_parameter("graph_params.useWindowForMarginalsComputation", false); + node_->declare_parameter("graph_params.windowSizeSecondsForMarginalsComputation", 0.0); + node_->declare_parameter("graph_params.createReferenceAlignmentKeyframeEveryNSeconds", 0.0); // Noise Params node_->declare_parameter("noise_params.accNoiseDensity", 0.0); From 3c9497097a08a52f0c0089494f337751f1f55774 Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Fri, 23 May 2025 12:47:39 +0000 Subject: [PATCH 24/60] Update parameter names and add new logging options in GraphMsfRos2 setup and readParams functions --- ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp | 5 ++++- ros2/graph_msf_ros2/src/lib/readParams.cpp | 2 -- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp index a5bf9191..75b39bab 100644 --- a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp +++ b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp @@ -55,7 +55,8 @@ void GraphMsfRos2::setup(std::shared_ptr staticTransformsPtr) node_->declare_parameter("graph_params.optimizeReferenceFramePosesWrtWorld", false); node_->declare_parameter("graph_params.optimizeExtrinsicSensorToSensorCorrectedOffset", false); node_->declare_parameter("graph_params.referenceFramePosesResetThreshold", 0.0); - // node_->declare_parameter("graph_params.centerReferenceFramesAtRobotPositionBeforeAlignment", false); + node_->declare_parameter("graph_params.centerMeasurementsAtKeyframePositionBeforeAlignment", false); + node_->declare_parameter("graph_params.useWindowForMarginalsComputation", false); node_->declare_parameter("graph_params.windowSizeSecondsForMarginalsComputation", 0.0); node_->declare_parameter("graph_params.createReferenceAlignmentKeyframeEveryNSeconds", 0.0); @@ -96,6 +97,8 @@ void GraphMsfRos2::setup(std::shared_ptr staticTransformsPtr) // Common Params node_->declare_parameter("common_params.verbosity", 0); node_->declare_parameter("common_params.odomNotJumpAtStart", false); + node_->declare_parameter("common_params.logRealTimeStateToMemory", false); + node_->declare_parameter("common_params.logLatencyAndUpdateDurationToMemory", false); // Extrinsic frames node_->declare_parameter("extrinsics.worldFrame", std::string("")); diff --git a/ros2/graph_msf_ros2/src/lib/readParams.cpp b/ros2/graph_msf_ros2/src/lib/readParams.cpp index 1891c99b..51c7685e 100644 --- a/ros2/graph_msf_ros2/src/lib/readParams.cpp +++ b/ros2/graph_msf_ros2/src/lib/readParams.cpp @@ -80,8 +80,6 @@ void GraphMsfRos2::readParams() { tryGetParam(node_, "graph_params.centerMeasurementsAtKeyframePositionBeforeAlignment"); graphConfigPtr_->createReferenceAlignmentKeyframeEveryNSeconds_ = tryGetParam(node_, "graph_params.createReferenceAlignmentKeyframeEveryNSeconds"); -// graphConfigPtr_->centerReferenceFramesAtRobotPositionBeforeAlignment_ = -// tryGetParam(node_, "graph_params.centerReferenceFramesAtRobotPositionBeforeAlignment"); // Noise Parameters graphConfigPtr_->accNoiseDensity_ = tryGetParam(node_, "noise_params.accNoiseDensity"); From c56e6158749ff2c6f73df4a69ed81377d76eaa10 Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Fri, 23 May 2025 13:15:17 +0000 Subject: [PATCH 25/60] Refactor YAML configuration files for smb_estimator_node to include parameters under ros__parameters section --- .../config/core/core_extrinsic_params.yaml | 28 ++-- .../config/core/core_graph_config.yaml | 14 +- .../config/core/core_graph_params.yaml | 152 +++++++++--------- .../smb_specific/smb_extrinsic_params.yaml | 16 +- .../config/smb_specific/smb_graph_params.yaml | 58 +++---- 5 files changed, 139 insertions(+), 129 deletions(-) diff --git a/examples/ros2/smb_estimator_graph_ros2/config/core/core_extrinsic_params.yaml b/examples/ros2/smb_estimator_graph_ros2/config/core/core_extrinsic_params.yaml index 669519d2..b512d3e4 100644 --- a/examples/ros2/smb_estimator_graph_ros2/config/core/core_extrinsic_params.yaml +++ b/examples/ros2/smb_estimator_graph_ros2/config/core/core_extrinsic_params.yaml @@ -1,14 +1,16 @@ -#External Prior/Transform Input -extrinsics: - # Published by GMSF - worldFrame: "world_graph_msf" - odomFrame: "odom_graph_msf" - # Used for estimation - imuFrame: "imu" - baseLinkFrame: "base_link" - initializeZeroYawAndPositionOfFrame: "rslidar" # Initialize the yaw and position of the base frame to zero - # this is just meant to avoid any jump after getting the first true measurements +/smb_estimator_node: + ros__parameters: + #External Prior/Transform Input + extrinsics: + # Published by GMSF + worldFrame: "world_graph_msf" + odomFrame: "odom_graph_msf" + # Used for estimation + imuFrame: "imu" + baseLinkFrame: "base_link" + initializeZeroYawAndPositionOfFrame: "rslidar" # Initialize the yaw and position of the base frame to zero + # this is just meant to avoid any jump after getting the first true measurements -name_ids: - referenceFrameAligned: "_graph_msf_aligned" # Specifies what is the suffix of the aligned published fixed frames - sensorFrameCorrected: "_graph_msf_corrected" # Specifies what is the suffix of the corrected published sensor frames + name_ids: + referenceFrameAligned: "_graph_msf_aligned" # Specifies what is the suffix of the aligned published fixed frames + sensorFrameCorrected: "_graph_msf_corrected" # Specifies what is the suffix of the corrected published sensor frames diff --git a/examples/ros2/smb_estimator_graph_ros2/config/core/core_graph_config.yaml b/examples/ros2/smb_estimator_graph_ros2/config/core/core_graph_config.yaml index b25fd342..79c42f89 100644 --- a/examples/ros2/smb_estimator_graph_ros2/config/core/core_graph_config.yaml +++ b/examples/ros2/smb_estimator_graph_ros2/config/core/core_graph_config.yaml @@ -1,6 +1,8 @@ -#Common -common_params: - verbosity: 0 #Debug Output, 0: only important events, 1: optimization duration, 2: added factors - odomNotJumpAtStart: false #Whether the World->Map transform should be updated in the beginning (odom start at identity) - logRealTimeStateToMemory: false # Whether the real-time state should be logged to memory (for logging purposes) - logLatencyAndUpdateDurationToMemory: false # Whether the latency and update duration should be logged to memory (for logging purposes) +/smb_estimator_node: + ros__parameters: + #Common + common_params: + verbosity: 0 #Debug Output, 0: only important events, 1: optimization duration, 2: added factors + odomNotJumpAtStart: false #Whether the World->Map transform should be updated in the beginning (odom start at identity) + logRealTimeStateToMemory: false # Whether the real-time state should be logged to memory (for logging purposes) + logLatencyAndUpdateDurationToMemory: false # Whether the latency and update duration should be logged to memory (for logging purposes) diff --git a/examples/ros2/smb_estimator_graph_ros2/config/core/core_graph_params.yaml b/examples/ros2/smb_estimator_graph_ros2/config/core/core_graph_params.yaml index 1d88f42d..0ded6873 100644 --- a/examples/ros2/smb_estimator_graph_ros2/config/core/core_graph_params.yaml +++ b/examples/ros2/smb_estimator_graph_ros2/config/core/core_graph_params.yaml @@ -1,79 +1,81 @@ -# Sensor Params -sensor_params: - imuRate: 200.0 # Rate of IMU input (Hz) - createStateEveryNthImuMeasurement: 4 # Create a new state every n-th IMU measurement - useImuSignalLowPassFilter: false # If true, a low pass filter is applied to the IMU measurements - imuLowPassFilterCutoffFreq: 30.0 # Cutoff frequency of the low pass filter - imuBufferLength: 800 - imuTimeOffset: 0.0 # Offset between IMU and LiDAR Measurements: can be determined with rqt_multiplot +/smb_estimator_node: + ros__parameters: + # Sensor Params + sensor_params: + imuRate: 200.0 # Rate of IMU input (Hz) + createStateEveryNthImuMeasurement: 4 # Create a new state every n-th IMU measurement + useImuSignalLowPassFilter: false # If true, a low pass filter is applied to the IMU measurements + imuLowPassFilterCutoffFreq: 30.0 # Cutoff frequency of the low pass filter + imuBufferLength: 800 + imuTimeOffset: 0.0 # Offset between IMU and LiDAR Measurements: can be determined with rqt_multiplot -# Initialization -initialization_params: - estimateGravityFromImu: false # If true, the gravity is estimated in the beginning using the IMU - gravityMagnitude: 9.80600 # If estimateGravityFromImu is false, this value is used as gravity + # Initialization + initialization_params: + estimateGravityFromImu: false # If true, the gravity is estimated in the beginning using the IMU + gravityMagnitude: 9.80600 # If estimateGravityFromImu is false, this value is used as gravity -# Factor Graph -graph_params: - realTimeSmootherLag: 2.0 # Lag of real-time fixed lag smoother[seconds] - realTimeSmootherUseIsam: true - realTimeSmootherUseCholeskyFactorization: true # CHOLESKY faster, QR numerically more stable - useAdditionalSlowBatchSmoother: true # If true, a slower smoother is used in addition to the real-time smoother - slowBatchSmootherUseIsam: false # if false, then levenberg-marquardt is used for the slow batch smoother, NO EFFECT YET - slowBatchSmootherUseCholeskyFactorization: false # CHOLESKY faster, QR numerically more stable - # Optimizer Config - gaussNewtonWildfireThreshold: 0.001 # Threshold for the wildfire in the Gauss-Newton optimization - minOptimizationFrequency: 1.0 # Minimum optimization frequency [Hz], makes sure that optimization is triggered at least every x Hz - maxOptimizationFrequency: 10.0 # Maximum optimization frequency [Hz], can be used to control computational load - additionalOptimizationIterations: 0 # Additional iterations of graph optimizer after update with new factors - findUnusedFactorSlots: true - enableDetailedResults: false - usingBiasForPreIntegration: true # If true, the bias is used during pre-integration - useWindowForMarginalsComputation: true # If true, the window is used for marginal computation - windowSizeSecondsForMarginalsComputation: 300.0 # Size of the window for marginal computation, unit: seconds - # Alignment Parameters - optimizeReferenceFramePosesWrtWorld: true # If true, the poses of the fixed frames are optimized w.r.t. world - referenceFramePosesResetThreshold: 8.0 # Reset T_M_W if distance between initial guess and optimization are too big, [SE(3) distance] - centerMeasurementsAtKeyframePositionBeforeAlignment: true # If true, the measurements are centered before alignment - createReferenceAlignmentKeyframeEveryNSeconds: 10.0 # Create a new keyframe for alignment every n seconds - # Extrinsic Calibration - optimizeExtrinsicSensorToSensorCorrectedOffset: false # If true, the extrinsic calibration is optimized + # Factor Graph + graph_params: + realTimeSmootherLag: 2.0 # Lag of real-time fixed lag smoother[seconds] + realTimeSmootherUseIsam: true + realTimeSmootherUseCholeskyFactorization: true # CHOLESKY faster, QR numerically more stable + useAdditionalSlowBatchSmoother: true # If true, a slower smoother is used in addition to the real-time smoother + slowBatchSmootherUseIsam: false # if false, then levenberg-marquardt is used for the slow batch smoother, NO EFFECT YET + slowBatchSmootherUseCholeskyFactorization: false # CHOLESKY faster, QR numerically more stable + # Optimizer Config + gaussNewtonWildfireThreshold: 0.001 # Threshold for the wildfire in the Gauss-Newton optimization + minOptimizationFrequency: 1.0 # Minimum optimization frequency [Hz], makes sure that optimization is triggered at least every x Hz + maxOptimizationFrequency: 10.0 # Maximum optimization frequency [Hz], can be used to control computational load + additionalOptimizationIterations: 0 # Additional iterations of graph optimizer after update with new factors + findUnusedFactorSlots: true + enableDetailedResults: false + usingBiasForPreIntegration: true # If true, the bias is used during pre-integration + useWindowForMarginalsComputation: true # If true, the window is used for marginal computation + windowSizeSecondsForMarginalsComputation: 300.0 # Size of the window for marginal computation, unit: seconds + # Alignment Parameters + optimizeReferenceFramePosesWrtWorld: true # If true, the poses of the fixed frames are optimized w.r.t. world + referenceFramePosesResetThreshold: 8.0 # Reset T_M_W if distance between initial guess and optimization are too big, [SE(3) distance] + centerMeasurementsAtKeyframePositionBeforeAlignment: true # If true, the measurements are centered before alignment + createReferenceAlignmentKeyframeEveryNSeconds: 10.0 # Create a new keyframe for alignment every n seconds + # Extrinsic Calibration + optimizeExtrinsicSensorToSensorCorrectedOffset: false # If true, the extrinsic calibration is optimized -# Noise Parameters -noise_params: - ## IMU - ### Position - accNoiseDensity: 2.2555e-04 # Continuous-time Noise Amplitude Spectral Density (StdDev) [m/s^2/√Hz)], default=sqrt(7.84e-06) - integrationNoiseDensity: 1.0e-04 # Continuous-time Noise Amplitude Spectral Density of integration uncertainty, default: sqrt(1.0e-07) - use2ndOrderCoriolis: false # Whether to use 2nd order coriolis effect - ### Rotation - gyrNoiseDensity: 2.356e-04 # Continuous-time Noise Amplitude Spectral Density (StdDev) [rad/s/√Hz], default=sqrt(3.4906585e-07) - omegaCoriolis: 1.07e-04 # Coriolis effect, positive on northern hemisphere, 0 at the equator, default (central europe, Switzerland): 1.07e-04 - ### Bias - accBiasRandomWalkNoiseDensity: 4.33e-03 # Continuous-time Noise Amplitude Spectral Density of Accelerometer bias random walk [m/s^3/√Hz], default: sqrt(1.0e-03) - gyrBiasRandomWalkNoiseDensity: 2.66e-04 # Continuous-time Noise Amplitude Spectral Density of Gyroscope bias random walk [rad/s^2/√Hz], default: default: sqrt(9.33e-04) - biasAccOmegaInit: 1.0e-05 # StdDev of bias at start: default: sqrt(1.0e-07) - accBiasPrior: 0.0 # Prior/starting value of accelerometer bias - gyrBiasPrior: 0.0 # Prior/starting value of gyroscope bias - ## Initial State - initialPositionNoiseDensity: 1.0e-04 # Prior/starting value of position - initialOrientationNoiseDensity: 1.0e-03 # Prior/starting value of orientation - initialVelocityNoiseDensity: 1.0e-02 # Prior/starting value of velocity - initialAccBiasNoiseDensity: 1.0e-03 # Prior/starting value of accelerometer bias - initialGyroBiasNoiseDensity: 1.0e-03 # Prior/starting value of gyroscope bias + # Noise Parameters + noise_params: + ## IMU + ### Position + accNoiseDensity: 2.2555e-04 # Continuous-time Noise Amplitude Spectral Density (StdDev) [m/s^2/√Hz)], default=sqrt(7.84e-06) + integrationNoiseDensity: 1.0e-04 # Continuous-time Noise Amplitude Spectral Density of integration uncertainty, default: sqrt(1.0e-07) + use2ndOrderCoriolis: false # Whether to use 2nd order coriolis effect + ### Rotation + gyrNoiseDensity: 2.356e-04 # Continuous-time Noise Amplitude Spectral Density (StdDev) [rad/s/√Hz], default=sqrt(3.4906585e-07) + omegaCoriolis: 1.07e-04 # Coriolis effect, positive on northern hemisphere, 0 at the equator, default (central europe, Switzerland): 1.07e-04 + ### Bias + accBiasRandomWalkNoiseDensity: 4.33e-03 # Continuous-time Noise Amplitude Spectral Density of Accelerometer bias random walk [m/s^3/√Hz], default: sqrt(1.0e-03) + gyrBiasRandomWalkNoiseDensity: 2.66e-04 # Continuous-time Noise Amplitude Spectral Density of Gyroscope bias random walk [rad/s^2/√Hz], default: default: sqrt(9.33e-04) + biasAccOmegaInit: 1.0e-05 # StdDev of bias at start: default: sqrt(1.0e-07) + accBiasPrior: 0.0 # Prior/starting value of accelerometer bias + gyrBiasPrior: 0.0 # Prior/starting value of gyroscope bias + ## Initial State + initialPositionNoiseDensity: 1.0e-04 # Prior/starting value of position + initialOrientationNoiseDensity: 1.0e-03 # Prior/starting value of orientation + initialVelocityNoiseDensity: 1.0e-02 # Prior/starting value of velocity + initialAccBiasNoiseDensity: 1.0e-03 # Prior/starting value of accelerometer bias + initialGyroBiasNoiseDensity: 1.0e-03 # Prior/starting value of gyroscope bias -# Relinearization -relinearization_params: - positionReLinTh: 1.0e-03 # Letter "x" in GTSAM variables, Position linearization threshold - rotationReLinTh: 1.0e-03 # Letter "x" in GTSAM variables, Rotation linearization threshold - velocityReLinTh: 1.0e-03 # Letter "v" in GTSAM variables, Linear Velocity linearization threshold - accBiasReLinTh: 1.0e-03 # Letter "b" in GTSAM variables, Accelerometer bias linearization threshold - gyrBiasReLinTh: 1.0e-03 # Letter "b" in GTSAM variables, Gyroscope bias linearization threshold - referenceFrameReLinTh: 1.0e-03 # Letter "r" in GTSAM variables, Reference frame linearization threshold, ONLY IF optimizeReferenceFramePosesWrtWorld - calibrationReLinTh: 1.0e-03 # Letter "c" in GTSAM variables, Calibration linearization threshold, ONLY IF optimizeExtrinsicSensorToSensorCorrectedOffset - displacementReLinTh: 1.0e-03 # Letter "d" in GTSAM variables, Displacement linearization threshold, ONLY IF optimizeExtrinsicSensorToSensorCorrectedOffset - landmarkReLinTh: 1.0e-03 # Letter "l" in GTSAM variables, Landmark linearization threshold - relinearizeSkip: 10 # Re-linearization is skipped every n-th step, default: 10 - enableRelinearization: true # Whether to use relinearization, default: true - evaluateNonlinearError: true # Whether to evaluate the nonlinear error before and after the update, default: false - cacheLinearizedFactors: true # Whether to cache the linearized factors, default: true - enablePartialRelinearizationCheck: false # Whether potentially only parts of the tree needs to be relinearized, default: false + # Relinearization + relinearization_params: + positionReLinTh: 1.0e-03 # Letter "x" in GTSAM variables, Position linearization threshold + rotationReLinTh: 1.0e-03 # Letter "x" in GTSAM variables, Rotation linearization threshold + velocityReLinTh: 1.0e-03 # Letter "v" in GTSAM variables, Linear Velocity linearization threshold + accBiasReLinTh: 1.0e-03 # Letter "b" in GTSAM variables, Accelerometer bias linearization threshold + gyrBiasReLinTh: 1.0e-03 # Letter "b" in GTSAM variables, Gyroscope bias linearization threshold + referenceFrameReLinTh: 1.0e-03 # Letter "r" in GTSAM variables, Reference frame linearization threshold, ONLY IF optimizeReferenceFramePosesWrtWorld + calibrationReLinTh: 1.0e-03 # Letter "c" in GTSAM variables, Calibration linearization threshold, ONLY IF optimizeExtrinsicSensorToSensorCorrectedOffset + displacementReLinTh: 1.0e-03 # Letter "d" in GTSAM variables, Displacement linearization threshold, ONLY IF optimizeExtrinsicSensorToSensorCorrectedOffset + landmarkReLinTh: 1.0e-03 # Letter "l" in GTSAM variables, Landmark linearization threshold + relinearizeSkip: 10 # Re-linearization is skipped every n-th step, default: 10 + enableRelinearization: true # Whether to use relinearization, default: true + evaluateNonlinearError: true # Whether to evaluate the nonlinear error before and after the update, default: false + cacheLinearizedFactors: true # Whether to cache the linearized factors, default: true + enablePartialRelinearizationCheck: false # Whether potentially only parts of the tree needs to be relinearized, default: false diff --git a/examples/ros2/smb_estimator_graph_ros2/config/smb_specific/smb_extrinsic_params.yaml b/examples/ros2/smb_estimator_graph_ros2/config/smb_specific/smb_extrinsic_params.yaml index 9175f676..765ec560 100644 --- a/examples/ros2/smb_estimator_graph_ros2/config/smb_specific/smb_extrinsic_params.yaml +++ b/examples/ros2/smb_estimator_graph_ros2/config/smb_specific/smb_extrinsic_params.yaml @@ -1,7 +1,9 @@ -#External Prior/Transform Input -extrinsics: - lidarOdometryFrame: "rslidar" #LiDAR frame name - used to lookup LiDAR-to-ExternalSensor Frame - wheelOdometryBetweenFrame: "base_link" #Wheel odometry frame name - used to lookup Wheel-to-ExternalSensor Frame - wheelLinearVelocityLeftFrame: "LH_WHEEL" #Wheel linear velocity left frame name - used to lookup Wheel-to-ExternalSensor Frame - wheelLinearVelocityRightFrame: "RH_WHEEL" #Wheel linear velocity right frame name - used to lookup Wheel-to-ExternalSensor Frame - vioOdometryFrame: "base_link" #VIO odometry frame name - used to lookup VIO-to-ExternalSensor Frame +/smb_estimator_node: + ros__parameters: + #External Prior/Transform Input + extrinsics: + lidarOdometryFrame: "rslidar" #LiDAR frame name - used to lookup LiDAR-to-ExternalSensor Frame + wheelOdometryBetweenFrame: "base_link" #Wheel odometry frame name - used to lookup Wheel-to-ExternalSensor Frame + wheelLinearVelocityLeftFrame: "LH_WHEEL" #Wheel linear velocity left frame name - used to lookup Wheel-to-ExternalSensor Frame + wheelLinearVelocityRightFrame: "RH_WHEEL" #Wheel linear velocity right frame name - used to lookup Wheel-to-ExternalSensor Frame + vioOdometryFrame: "base_link" #VIO odometry frame name - used to lookup VIO-to-ExternalSensor Frame diff --git a/examples/ros2/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml b/examples/ros2/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml index 7ef5c4df..a816777f 100644 --- a/examples/ros2/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml +++ b/examples/ros2/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml @@ -1,30 +1,32 @@ -# Sensor Params -sensor_params: - ## Config - useLioOdometry: true - useWheelOdometryBetween: false - useWheelLinearVelocities: false - useVioOdometry: false - ## Rates - lioOdometryRate: 10 - wheelOdometryBetweenRate: 50 - wheelLinearVelocitiesRate: 50 - vioOdometryRate: 50 - # Wheel Radius - wheelRadius: 0.195 # meters +/smb_estimator_node: + ros__parameters: + # Sensor Params + sensor_params: + ## Config + useLioOdometry: true + useWheelOdometryBetween: false + useWheelLinearVelocities: false + useVioOdometry: false + ## Rates + lioOdometryRate: 10 + wheelOdometryBetweenRate: 50 + wheelLinearVelocitiesRate: 50 + vioOdometryRate: 50 + # Wheel Radius + wheelRadius: 0.195 # meters -# Alignment Parameters -alignment_params: - initialSe3AlignmentNoiseDensity: [ 1.0e-01, 1.0e-01, 1.0e-01, 1.0e-02, 1.0e-02, 1.0e-02 ] # StdDev, ORDER RPY(rad) - XYZ(meters) - lioSe3AlignmentRandomWalk: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + # Alignment Parameters + alignment_params: + initialSe3AlignmentNoiseDensity: [ 1.0e-01, 1.0e-01, 1.0e-01, 1.0e-02, 1.0e-02, 1.0e-02 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + lioSe3AlignmentRandomWalk: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] # StdDev, ORDER RPY(rad) - XYZ(meters) -# Noise Parameters -noise_params: - ## LiDAR - lioPoseUnaryNoiseDensity: [ 0.05, 0.05, 0.05, 0.01, 0.01, 0.01 ] # StdDev, ORDER RPY(rad) - XYZ(meters) - ## Wheel - wheelPoseBetweenNoiseDensity: [ 0.1, 0.1, 0.1, 0.06, 0.06, 0.06 ] # StdDev, ORDER RPY(rad) - XYZ(meters) - wheelLinearVelocitiesNoiseDensity: [ 0.3, 0.3, 0.3 ] # StdDev, ORDER VxVyVz(meters/sec) - ## VIO - vioPoseBetweenNoiseDensity: [ 100.0, 100.0, 100.0, 0.1, 0.1, 0.1 ] # StdDev, ORDER RPY(rad) - XYZ(meters) - vioPoseUnaryNoiseDensity: [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + # Noise Parameters + noise_params: + ## LiDAR + lioPoseUnaryNoiseDensity: [ 0.05, 0.05, 0.05, 0.01, 0.01, 0.01 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + ## Wheel + wheelPoseBetweenNoiseDensity: [ 0.1, 0.1, 0.1, 0.06, 0.06, 0.06 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + wheelLinearVelocitiesNoiseDensity: [ 0.3, 0.3, 0.3 ] # StdDev, ORDER VxVyVz(meters/sec) + ## VIO + vioPoseBetweenNoiseDensity: [ 100.0, 100.0, 100.0, 0.1, 0.1, 0.1 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + vioPoseUnaryNoiseDensity: [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 ] # StdDev, ORDER RPY(rad) - XYZ(meters) From 3438fd299bb4c4432726f7a3031bd3bae4584bc2 Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Fri, 23 May 2025 15:15:02 +0000 Subject: [PATCH 26/60] Add launch file for smb_estimator_graph_ros2 to configure simulation parameters and node execution --- .../launch/smb_estimator_graph_sim.launch.py | 92 +++++++++++++++++++ 1 file changed, 92 insertions(+) create mode 100644 examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph_sim.launch.py diff --git a/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph_sim.launch.py b/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph_sim.launch.py new file mode 100644 index 00000000..9ccbd6ed --- /dev/null +++ b/examples/ros2/smb_estimator_graph_ros2/launch/smb_estimator_graph_sim.launch.py @@ -0,0 +1,92 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + # Package name + pkg_name = 'smb_estimator_graph_ros2' + + # Get package share directory + pkg_dir = get_package_share_directory(pkg_name) + + # Declare launch arguments + use_sim_time = LaunchConfiguration('use_sim_time') + imu_topic_name = LaunchConfiguration('imu_topic_name') + lidar_odometry_topic_name = LaunchConfiguration('lidar_odometry_topic_name') + wheel_odometry_topic_name = LaunchConfiguration('wheel_odometry_topic_name') + wheel_velocities_topic_name = LaunchConfiguration('wheel_velocities_topic_name') + vio_odometry_topic_name = LaunchConfiguration('vio_odometry_topic_name') + logging_dir_location = LaunchConfiguration('logging_dir_location') + + # Default config file paths + core_graph_config_param_file = os.path.join(pkg_dir, 'config', 'core', 'core_graph_config.yaml') + core_graph_param_file = os.path.join(pkg_dir, 'config', 'core', 'core_graph_params.yaml') + core_extrinsic_param_file = os.path.join(pkg_dir, 'config', 'core', 'core_extrinsic_params.yaml') + smb_graph_param_file = os.path.join(pkg_dir, 'config', 'smb_specific', 'smb_graph_params.yaml') + smb_extrinsic_param_file = os.path.join(pkg_dir, 'config', 'smb_specific', 'smb_extrinsic_params.yaml') + + return LaunchDescription([ + # Declare launch arguments + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation time' + ), + DeclareLaunchArgument( + 'imu_topic_name', + default_value='/imu', + description='IMU topic name' + ), + DeclareLaunchArgument( + 'lidar_odometry_topic_name', + default_value='/dlio/odom_node/odom', + description='Lidar odometry topic name' + ), + DeclareLaunchArgument( + 'wheel_odometry_topic_name', + default_value='/control/smb_diff_drive/odom', + description='Wheel odometry topic name' + ), + DeclareLaunchArgument( + 'wheel_velocities_topic_name', + default_value='/control/smb_lowlevel_controller/wheelSpeeds', + description='Wheel velocities topic name' + ), + DeclareLaunchArgument( + 'vio_odometry_topic_name', + default_value='/tracking_camera/odom/sample', + description='VIO odometry topic name' + ), + DeclareLaunchArgument( + 'logging_dir_location', + default_value=os.path.join(pkg_dir, 'logging'), + description='Logging directory location' + ), + + # Node + Node( + package='smb_estimator_graph_ros2', + executable='smb_estimator_graph_ros2_node', + name='smb_estimator_node', + output='screen', + parameters=[ + {'use_sim_time': use_sim_time}, + {'launch/optimizationResultLoggingPath': logging_dir_location}, + core_graph_config_param_file, + core_graph_param_file, + core_extrinsic_param_file, + smb_graph_param_file, + smb_extrinsic_param_file + ], + remappings=[ + ('/imu_topic', imu_topic_name), + ('/lidar_odometry_topic', lidar_odometry_topic_name), + ('/wheel_odometry_topic', wheel_odometry_topic_name), + ('/wheel_velocities_topic', wheel_velocities_topic_name), + ('/vio_odometry_topic', vio_odometry_topic_name) + ] + ) + ]) \ No newline at end of file From 6ff2d25e0e320ed363a72aa475e7cd18dff38636 Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Fri, 23 May 2025 15:15:20 +0000 Subject: [PATCH 27/60] Update IMU frame parameter in core_extrinsic_params.yaml from 'imu' to 'imu_link' --- .../config/core/core_extrinsic_params.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/ros2/smb_estimator_graph_ros2/config/core/core_extrinsic_params.yaml b/examples/ros2/smb_estimator_graph_ros2/config/core/core_extrinsic_params.yaml index b512d3e4..b42c84dd 100644 --- a/examples/ros2/smb_estimator_graph_ros2/config/core/core_extrinsic_params.yaml +++ b/examples/ros2/smb_estimator_graph_ros2/config/core/core_extrinsic_params.yaml @@ -6,7 +6,7 @@ worldFrame: "world_graph_msf" odomFrame: "odom_graph_msf" # Used for estimation - imuFrame: "imu" + imuFrame: "imu_link" baseLinkFrame: "base_link" initializeZeroYawAndPositionOfFrame: "rslidar" # Initialize the yaw and position of the base frame to zero # this is just meant to avoid any jump after getting the first true measurements From 7429b29172dc58138f2044fa622dc849f54c4ddf Mon Sep 17 00:00:00 2001 From: Julian Date: Fri, 23 May 2025 22:34:45 +0200 Subject: [PATCH 28/60] first almost compiling version, just issue with one symbol and libmetis --- .../ros/anymal_estimator_graph/CMakeLists.txt | 11 +--- graph_msf/CMakeLists.txt | 45 ++++++++++++++- ros/graph_msf_catkin/CMakeLists.txt | 55 +++++++++++-------- ros/graph_msf_ros/CMakeLists.txt | 5 +- ros/graph_msf_ros/package.xml | 4 +- 5 files changed, 81 insertions(+), 39 deletions(-) diff --git a/examples/ros/anymal_estimator_graph/CMakeLists.txt b/examples/ros/anymal_estimator_graph/CMakeLists.txt index 53699a28..f751873d 100644 --- a/examples/ros/anymal_estimator_graph/CMakeLists.txt +++ b/examples/ros/anymal_estimator_graph/CMakeLists.txt @@ -21,14 +21,6 @@ if (NOT WIN32) set(Magenta "${Esc}[35m") endif () -# March native check -#include(CheckCXXCompilerFlag) -#CHECK_CXX_COMPILER_FLAG("-march=native" COMPILER_SUPPORTS_MARCH_NATIVE) -#if(COMPILER_SUPPORTS_MARCH_NATIVE) -# add_compile_options("-march=native") -# message("${BoldMagenta}INFO: Using -march=native${ColourReset}") -#endif() - # Catkin dependencies ------------------------------------------------------------------------------------------------- set(CATKIN_PACKAGE_DEPENDENCIES graph_msf_ros @@ -41,12 +33,14 @@ set(CATKIN_PACKAGE_DEPENDENCIES graph_msf_anymal_msgs ) +# CMAKE dependencies -------------------------------------------------------------------------------- find_package(catkin REQUIRED COMPONENTS ${CATKIN_PACKAGE_DEPENDENCIES} ) find_package(Glog REQUIRED) +# Catkin Package catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} @@ -104,6 +98,7 @@ endif (cmake_clang_tools_FOUND AND NOT DEFINED NO_CLANG_TOOLING) ############# ## Install ## ############# + install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} diff --git a/graph_msf/CMakeLists.txt b/graph_msf/CMakeLists.txt index c3640e04..9944aab4 100644 --- a/graph_msf/CMakeLists.txt +++ b/graph_msf/CMakeLists.txt @@ -1,9 +1,25 @@ cmake_minimum_required(VERSION 3.16) project(graph_msf) +message("Building graph_msf library -----------------------------") + ## Compile as C++17, supported in ROS Noetic and newer add_compile_options(-std=c++17) +# If build variables not set, set them to default values +if (NOT DEFINED CMAKE_BUILD_LIBDIR) + set(CMAKE_BUILD_LIBDIR ${CMAKE_BINARY_DIR}/lib) +endif () +if (NOT DEFINED CMAKE_BUILD_BINDIR) + set(CMAKE_BUILD_BINDIR ${CMAKE_BINARY_DIR}/bin) +endif () +if (NOT DEFINED CMAKE_BUILD_INCLUDE_DIR) + set(CMAKE_BUILD_INCLUDE_DIR ${CMAKE_BINARY_DIR}/include) +endif () +message("CMAKE_BUILD_LIBDIR: ${CMAKE_BUILD_LIBDIR}") +message("CMAKE_BUILD_BINDIR: ${CMAKE_BUILD_BINDIR}") +message("CMAKE_BUILD_INCLUDE_DIR: ${CMAKE_BUILD_INCLUDE_DIR}") + # Find dependencies ---------------------------------------------------------------------------------------------------- find_package(Eigen3 REQUIRED) message("Eigen Version::" ${EIGEN3_VERSION_STRING}) @@ -21,6 +37,9 @@ endif () find_package(Eigen3 REQUIRED COMPONENTS) # GTSAM find_package(GTSAM REQUIRED) +find_package(GTSAM_UNSTABLE REQUIRED) +message("GTSAM Version:" ${GTSAM_VERSION}) +message("GTSAM Path:" ${GTSAM_DIR}) ########### ## Build ## @@ -34,7 +53,7 @@ include_directories( ${GTSAM_INCLUDE_DIR} ) -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/lib/eigen_wrapped_gtsam_utils.cpp src/lib/FileLogger.cpp src/lib/GraphMsf.cpp @@ -50,9 +69,11 @@ add_library(${PROJECT_NAME} src/lib/NavState.cpp ) +# Link GTSAM and other dependencies to the graph_msf library target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - ${GTSAM_LIBRARIES} + gtsam + gtsam_unstable + libmetis-gtsam.so ) # Add clang tooling @@ -70,12 +91,30 @@ endif (cmake_clang_tools_FOUND AND NOT DEFINED NO_CLANG_TOOLING) ## Install ## ############# +# Export the include directories and linked libraries install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}-targets # Export the target for downstream usage ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} ) +# Install the export file for downstream projects +install(EXPORT ${PROJECT_NAME}-targets + FILE ${PROJECT_NAME}Config.cmake + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME} +) + +# Install the include directory install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/${PROJECT_NAME} ) + +# Add GTSAM include directories to the exported interface +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + ${EIGEN3_INCLUDE_DIR} +) + +message("Finished building graph_msf library -----------------------------") diff --git a/ros/graph_msf_catkin/CMakeLists.txt b/ros/graph_msf_catkin/CMakeLists.txt index ad38e95f..2dcbbe30 100644 --- a/ros/graph_msf_catkin/CMakeLists.txt +++ b/ros/graph_msf_catkin/CMakeLists.txt @@ -1,15 +1,18 @@ cmake_minimum_required(VERSION 3.10) project(graph_msf_catkin) +## Compile as C++17, supported in ROS Noetic and newer +add_compile_options(-std=c++17) + find_package(catkin REQUIRED) # Color -if (NOT WIN32) +if(NOT WIN32) string(ASCII 27 Esc) set(ColourReset "${Esc}[m") set(BoldMagenta "${Esc}[1;35m") set(Magenta "${Esc}[35m") -endif () +endif() # Print CMAKE_BINARY_DIR message("CMAKE_BINARY_DIR: ${CMAKE_BINARY_DIR}") @@ -21,37 +24,40 @@ message("CATKIN_GLOBAL_INCLUDE_DESTINATION: ${CATKIN_GLOBAL_INCLUDE_DESTINATION} # Print CATKIN_GLOBAL_LIB_DESTINATION message("CATKIN_GLOBAL_LIB_DESTINATION: ${CATKIN_GLOBAL_LIB_DESTINATION}") -# Graph MSF -include(ExternalProject) -file(MAKE_DIRECTORY ${CATKIN_DEVEL_PREFIX}/include) +# Ensure CATKIN_DEVEL_PREFIX is set, otherwise use a fallback +if(NOT CATKIN_DEVEL_PREFIX) + set(CATKIN_DEVEL_PREFIX "${CMAKE_BINARY_DIR}/devel") +endif() -ExternalProject_Add(graph_msf - SOURCE_DIR ${CMAKE_SOURCE_DIR}/../../graph_msf - PREFIX "${CMAKE_SOURCE_DIR}" - TMP_DIR "${CMAKE_BINARY_DIR}/graph_msf/tmp" - BINARY_DIR "${CMAKE_BINARY_DIR}/graph_msf/build" - INSTALL_DIR "${CATKIN_DEVEL_PREFIX}" - CMAKE_CACHE_ARGS "-DCMAKE_POSITION_INDEPENDENT_CODE:BOOL=true" - CMAKE_ARGS - -DCMAKE_BUILD_TYPE=Release - -DCMAKE_INSTALL_PREFIX=${CATKIN_DEVEL_PREFIX} - BUILD_ALWAYS 1 +# Add the non-Catkin package ---------------------------------------------- +file(MAKE_DIRECTORY ${CATKIN_DEVEL_PREFIX}/include) +add_subdirectory(${CMAKE_SOURCE_DIR}/../../graph_msf + ${CATKIN_DEVEL_PREFIX}/lib ) -# Dependencies --------------------------------------------------------- -add_dependencies(graph_msf ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) +# Move the include directory to the correct location +add_custom_target(copy_graph_msf ALL + # Include + COMMAND ${CMAKE_COMMAND} -E make_directory ${CATKIN_DEVEL_PREFIX}/include + COMMAND ${CMAKE_COMMAND} -E copy_directory + "${CMAKE_SOURCE_DIR}/../../graph_msf/include/graph_msf" + "${CATKIN_DEVEL_PREFIX}/include/graph_msf" +) ############# ## INSTALL ## ############# -install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/include/ - DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} - ) +# Install the include directory +install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/.private/graph_msf_catkin/include + DESTINATION ${CATKIN_DEVEL_PREFIX}/include/graph_msf +) -install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/lib/ - DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION} - ) +# Install the library +install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/.private/graph_msf_catkin/lib + DESTINATION ${CATKIN_DEVEL_PREFIX}/lib + FILES_MATCHING PATTERN "*.so" +) # Final catkin package -------------------------------------------------- catkin_package( @@ -64,3 +70,4 @@ catkin_package( ${${PROJECT_NAME}_CATKIN_RUN_DEPENDS} ${CATKIN_PACKAGE_DEPENDENCIES} ) + diff --git a/ros/graph_msf_ros/CMakeLists.txt b/ros/graph_msf_ros/CMakeLists.txt index 8b24d83a..ea210f41 100644 --- a/ros/graph_msf_ros/CMakeLists.txt +++ b/ros/graph_msf_ros/CMakeLists.txt @@ -28,7 +28,7 @@ endif () # Catkin set(CATKIN_PACKAGE_DEPENDENCIES roscpp - graph_msf + graph_msf_catkin kdl_parser nav_msgs geometry_msgs @@ -78,8 +78,9 @@ add_library(${PROJECT_NAME} src/lib/GraphMsfRos.cpp src/lib/readParams.cpp src/lib/conversions.cpp) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) +# Link against graph_msf and its dependencies +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) # Add clang tooling diff --git a/ros/graph_msf_ros/package.xml b/ros/graph_msf_ros/package.xml index c757da8e..53f31509 100644 --- a/ros/graph_msf_ros/package.xml +++ b/ros/graph_msf_ros/package.xml @@ -15,7 +15,7 @@ catkin roscpp rospy - graph_msf + graph_msf_catkin kdl_parser nav_msgs geometry_msgs @@ -26,7 +26,7 @@ roscpp rospy python3 - graph_msf + graph_msf_catkin kdl_parser nav_msgs geometry_msgs From 320cf14290848c2b010651df7519239e040371c8 Mon Sep 17 00:00:00 2001 From: Julian Date: Mon, 26 May 2025 20:45:05 +0200 Subject: [PATCH 29/60] running version, full separation of graph msf and build system --- graph_msf/CMakeLists.txt | 2 +- ros/graph_msf_catkin/CMakeLists.txt | 7 +++++++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/graph_msf/CMakeLists.txt b/graph_msf/CMakeLists.txt index 9944aab4..3a6dafbf 100644 --- a/graph_msf/CMakeLists.txt +++ b/graph_msf/CMakeLists.txt @@ -73,7 +73,7 @@ add_library(${PROJECT_NAME} SHARED target_link_libraries(${PROJECT_NAME} gtsam gtsam_unstable - libmetis-gtsam.so + metis-gtsam ) # Add clang tooling diff --git a/ros/graph_msf_catkin/CMakeLists.txt b/ros/graph_msf_catkin/CMakeLists.txt index 2dcbbe30..b7c6b598 100644 --- a/ros/graph_msf_catkin/CMakeLists.txt +++ b/ros/graph_msf_catkin/CMakeLists.txt @@ -44,6 +44,10 @@ add_custom_target(copy_graph_msf ALL "${CATKIN_DEVEL_PREFIX}/include/graph_msf" ) +# GTSAM --> Also expose +find_package(GTSAM REQUIRED) +find_package(GTSAM_UNSTABLE REQUIRED) + ############# ## INSTALL ## ############# @@ -66,6 +70,9 @@ catkin_package( LIBRARIES ${CATKIN_DEVEL_PREFIX}/lib/libgraph_msf.so ${CS_PROJECT_LIBRARIES} + gtsam + gtsam_unstable + metis-gtsam CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_RUN_DEPENDS} ${CATKIN_PACKAGE_DEPENDENCIES} From a2d69fc42ba242fd45893404e7dec7c268a7fd8f Mon Sep 17 00:00:00 2001 From: Julian Date: Mon, 26 May 2025 21:02:44 +0200 Subject: [PATCH 30/60] moving of examples inside ros and ros2 folders --- examples/README.md | 44 ------ .../gtsam_tryout_examples/CMakeLists.txt | 108 --------------- .../gtsam_tryout_examples/package.xml | 37 ----- .../src/alignment_tryout_main.cpp | 85 ------------ .../fixed_lag_smoother_invalid_key_main.cpp | 128 ------------------ examples/ros2/README.md | 1 - .../anymal_estimator_graph/CMakeLists.txt | 0 .../anymal_estimator_graph/FindGflags.cmake | 0 .../anymal_estimator_graph/FindGlog.cmake | 0 .../anymal_estimator_graph/README.md | 0 .../anymal_extrinsic_params.yaml | 0 .../anymal_specific/anymal_gnss_params.yaml | 0 .../anymal_graph_params_gnss.yaml | 0 .../anymal_graph_params_no_gnss.yaml | 0 .../anymal_traj_align_params.yaml | 0 .../config/core/core_extrinsic_params.yaml | 0 .../config/core/core_graph_config.yaml | 0 .../config/core/core_graph_params_gnss.yaml | 0 .../core/core_graph_params_no_gnss.yaml | 0 ...1-13-16-27-14_anymal-d020-lpc_mission.yaml | 0 .../config/elevation_mapping/default.yaml | 0 .../elevation_mapping/offline_vlp16.yaml | 0 .../elevation_mapping/online_vlp16.yaml | 0 .../anymal_estimator_graph/AnymalEstimator.h | 0 .../AnymalStaticTransforms.h | 0 .../anymal_estimator_graph/constants.h | 0 .../launch/anymal_estimator_graph.launch | 0 .../anymal_estimator_graph_replay.launch | 0 ..._estimator_graph_replay_offline_bag.launch | 0 ...stimator_graph_replay_with_compslam.launch | 0 ...stimator_graph_replay_with_o3d_slam.launch | 0 .../anymal_estimator_graph/package.xml | 0 .../rviz/gnss_lidar.rviz | 0 .../rviz/gnss_lidar_offline.rviz | 0 .../src/anymal_estimator_node.cpp | 0 .../src/lib/AnymalEstimator.cpp | 0 .../src/lib/AnymalStaticTransforms.cpp | 0 .../src/lib/readParams.cpp | 0 .../atn_position3_fuser/CMakeLists.txt | 0 .../atn_position3_fuser/FindGflags.cmake | 0 .../atn_position3_fuser/FindGlog.cmake | 0 .../examples}/atn_position3_fuser/README.md | 0 .../config/core/core_extrinsic_params.yaml | 0 .../config/core/core_graph_config.yaml | 0 .../config/core/core_graph_params.yaml | 0 .../config/elevation_mapping/default.yaml | 0 .../elevation_mapping/offline_vlp16.yaml | 0 .../elevation_mapping/online_vlp16.yaml | 0 .../position_graph_extrinsic_params.yaml | 0 .../position_graph_graph_params.yaml | 0 .../trajectory_alignment_params.yaml | 0 .../atn_position3_fuser/Position3Estimator.h | 0 .../Position3StaticTransforms.h | 0 .../include/atn_position3_fuser/constants.h | 0 .../launch/position3_fuser.launch | 0 .../launch/position3_fuser_replay.launch | 0 .../position3_fuser_replay_offline_bag.launch | 0 .../examples}/atn_position3_fuser/package.xml | 0 .../position3_fuser_py/check_gnss_in_bag.py | 0 .../rviz/rviz_imuOdom.rviz | 0 .../rviz/rviz_imuOdom_offline.rviz | 0 .../examples}/atn_position3_fuser/setup.py | 0 .../atn_position3_fuser/src/graph_node.cpp | 0 .../src/lib/Position3Estimator.cpp | 0 .../src/lib/Position3StaticTransforms.cpp | 0 .../src/lib/readParams.cpp | 0 .../excavator_holistic_graph/CMakeLists.txt | 0 .../config/core/core_extrinsic_params.yaml | 0 .../config/core/core_graph_config.yaml | 0 .../config/core/core_graph_params.yaml | 0 .../excavator_extrinsic_params.yaml | 0 .../excavator_gnss_params.yaml | 0 .../excavator_graph_params.yaml | 0 .../ExcavatorEstimator.h | 0 .../ExcavatorStaticTransforms.h | 0 .../excavator_holistic_graph/constants.h | 0 .../launch/graph.launch | 0 .../launch/graph_no_urdf.launch | 0 .../excavator_holistic_graph/package.xml | 0 .../rviz/gnss_lidar.rviz | 0 .../scripts/remove_map_and_odom_from_bag.py | 0 .../src/graph_node.cpp | 0 .../src/lib/ExcavatorEstimator.cpp | 0 .../src/lib/ExcavatorStaticTransforms.cpp | 0 .../src/lib/readParams.cpp | 0 .../graph_msf_ros_examples/CMakeLists.txt | 0 .../graph_msf_ros_examples/package.xml | 0 .../examples}/imu_pose3_fuser/CMakeLists.txt | 0 .../config/core/core_extrinsic_params.yaml | 0 .../config/core/core_graph_config.yaml | 0 .../config/core/core_graph_params.yaml | 0 .../imu_pose3_fuser_extrinsic_params.yaml | 0 .../specific/imu_pose3_fuser_params.yaml | 0 .../include/imu_pose3_fuser/ImuPose3Fuser.h | 0 .../include/imu_pose3_fuser/constants.h | 0 .../launch/imu_pose3_fuser.launch | 0 .../launch/imu_pose3_fuser_replay.launch | 0 .../examples}/imu_pose3_fuser/package.xml | 0 .../imu_pose3_fuser/rviz/estimation.rviz | 0 .../src/imu_pose3_fuser_node.cpp | 0 .../imu_pose3_fuser/src/lib/ImuPose3Fuser.cpp | 0 .../imu_pose3_fuser/src/lib/readParams.cpp | 0 .../pure_imu_integration/CMakeLists.txt | 0 .../pure_imu_integration/ImuIntegrator.h | 0 .../launch/dead_reckoning.launch | 0 .../launch/dead_reckoning_replay.launch | 0 .../pure_imu_integration/package.xml | 0 .../rviz/imu_integration.rviz | 0 .../src/lib/ImuIntegrator.cpp | 0 .../src/pure_imu_integration_node.cpp | 0 .../smb_estimator_graph/CMakeLists.txt | 0 .../smb_estimator_graph/FindGflags.cmake | 0 .../smb_estimator_graph/FindGlog.cmake | 0 .../compslam/compslam_lio_RS16_params.yaml | 0 .../config/core/core_extrinsic_params.yaml | 0 .../config/core/core_graph_config.yaml | 0 .../config/core/core_graph_params.yaml | 0 .../dumped_params/2023-06-13-13-15-57.yaml | 0 .../smb_specific/smb_extrinsic_params.yaml | 0 .../config/smb_specific/smb_graph_params.yaml | 0 .../smb_estimator_graph/SmbEstimator.h | 0 .../smb_estimator_graph/SmbStaticTransforms.h | 0 .../include/smb_estimator_graph/constants.h | 0 .../launch/smb_estimator_graph.launch | 0 .../launch/smb_estimator_graph_replay.launch | 0 .../examples}/smb_estimator_graph/package.xml | 0 .../rviz/lidar_estimation.rviz | 0 .../scripts/remove_map_and_odom_from_bag.py | 0 .../src/lib/SmbEstimator.cpp | 0 .../src/lib/SmbStaticTransforms.cpp | 0 .../src/lib/readParams.cpp | 0 .../src/smb_estimator_node.cpp | 0 ros2/examples/README.md | 1 + 133 files changed, 1 insertion(+), 403 deletions(-) delete mode 100644 examples/README.md delete mode 100644 examples/_playground/gtsam_tryout_examples/CMakeLists.txt delete mode 100644 examples/_playground/gtsam_tryout_examples/package.xml delete mode 100644 examples/_playground/gtsam_tryout_examples/src/alignment_tryout_main.cpp delete mode 100644 examples/_playground/gtsam_tryout_examples/src/fixed_lag_smoother_invalid_key_main.cpp delete mode 100644 examples/ros2/README.md rename {examples/ros => ros/examples}/anymal_estimator_graph/CMakeLists.txt (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/FindGflags.cmake (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/FindGlog.cmake (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/README.md (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/config/anymal_specific/anymal_extrinsic_params.yaml (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/config/anymal_specific/anymal_gnss_params.yaml (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/config/anymal_specific/anymal_graph_params_gnss.yaml (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/config/anymal_specific/anymal_graph_params_no_gnss.yaml (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/config/anymal_specific/anymal_traj_align_params.yaml (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/config/core/core_extrinsic_params.yaml (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/config/core/core_graph_config.yaml (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/config/core/core_graph_params_gnss.yaml (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/config/core/core_graph_params_no_gnss.yaml (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/config/dumped_params_d/2023-01-13-16-27-14_anymal-d020-lpc_mission.yaml (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/config/elevation_mapping/default.yaml (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/config/elevation_mapping/offline_vlp16.yaml (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/config/elevation_mapping/online_vlp16.yaml (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/include/anymal_estimator_graph/AnymalEstimator.h (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/include/anymal_estimator_graph/AnymalStaticTransforms.h (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/include/anymal_estimator_graph/constants.h (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/launch/anymal_estimator_graph.launch (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/launch/anymal_estimator_graph_replay.launch (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/launch/anymal_estimator_graph_replay_offline_bag.launch (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/launch/anymal_estimator_graph_replay_with_compslam.launch (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/launch/anymal_estimator_graph_replay_with_o3d_slam.launch (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/package.xml (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/rviz/gnss_lidar.rviz (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/rviz/gnss_lidar_offline.rviz (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/src/anymal_estimator_node.cpp (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/src/lib/AnymalEstimator.cpp (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/src/lib/AnymalStaticTransforms.cpp (100%) rename {examples/ros => ros/examples}/anymal_estimator_graph/src/lib/readParams.cpp (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/CMakeLists.txt (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/FindGflags.cmake (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/FindGlog.cmake (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/README.md (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/config/core/core_extrinsic_params.yaml (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/config/core/core_graph_config.yaml (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/config/core/core_graph_params.yaml (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/config/elevation_mapping/default.yaml (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/config/elevation_mapping/offline_vlp16.yaml (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/config/elevation_mapping/online_vlp16.yaml (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/config/position_graph_specific/position_graph_extrinsic_params.yaml (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/config/position_graph_specific/position_graph_graph_params.yaml (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/config/position_graph_specific/trajectory_alignment_params.yaml (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/include/atn_position3_fuser/Position3Estimator.h (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/include/atn_position3_fuser/Position3StaticTransforms.h (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/include/atn_position3_fuser/constants.h (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/launch/position3_fuser.launch (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/launch/position3_fuser_replay.launch (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/launch/position3_fuser_replay_offline_bag.launch (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/package.xml (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/position3_fuser_py/check_gnss_in_bag.py (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/rviz/rviz_imuOdom.rviz (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/rviz/rviz_imuOdom_offline.rviz (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/setup.py (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/src/graph_node.cpp (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/src/lib/Position3Estimator.cpp (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/src/lib/Position3StaticTransforms.cpp (100%) rename {examples/ros => ros/examples}/atn_position3_fuser/src/lib/readParams.cpp (100%) rename {examples/ros => ros/examples}/excavator_holistic_graph/CMakeLists.txt (100%) rename {examples/ros => ros/examples}/excavator_holistic_graph/config/core/core_extrinsic_params.yaml (100%) rename {examples/ros => ros/examples}/excavator_holistic_graph/config/core/core_graph_config.yaml (100%) rename {examples/ros => ros/examples}/excavator_holistic_graph/config/core/core_graph_params.yaml (100%) rename {examples/ros => ros/examples}/excavator_holistic_graph/config/excavator_specific/excavator_extrinsic_params.yaml (100%) rename {examples/ros => ros/examples}/excavator_holistic_graph/config/excavator_specific/excavator_gnss_params.yaml (100%) rename {examples/ros => ros/examples}/excavator_holistic_graph/config/excavator_specific/excavator_graph_params.yaml (100%) rename {examples/ros => ros/examples}/excavator_holistic_graph/include/excavator_holistic_graph/ExcavatorEstimator.h (100%) rename {examples/ros => ros/examples}/excavator_holistic_graph/include/excavator_holistic_graph/ExcavatorStaticTransforms.h (100%) rename {examples/ros => ros/examples}/excavator_holistic_graph/include/excavator_holistic_graph/constants.h (100%) rename {examples/ros => ros/examples}/excavator_holistic_graph/launch/graph.launch (100%) rename {examples/ros => ros/examples}/excavator_holistic_graph/launch/graph_no_urdf.launch (100%) rename {examples/ros => ros/examples}/excavator_holistic_graph/package.xml (100%) rename {examples/ros => ros/examples}/excavator_holistic_graph/rviz/gnss_lidar.rviz (100%) rename {examples/ros => ros/examples}/excavator_holistic_graph/scripts/remove_map_and_odom_from_bag.py (100%) rename {examples/ros => ros/examples}/excavator_holistic_graph/src/graph_node.cpp (100%) rename {examples/ros => ros/examples}/excavator_holistic_graph/src/lib/ExcavatorEstimator.cpp (100%) rename {examples/ros => ros/examples}/excavator_holistic_graph/src/lib/ExcavatorStaticTransforms.cpp (100%) rename {examples/ros => ros/examples}/excavator_holistic_graph/src/lib/readParams.cpp (100%) rename {examples/ros => ros/examples}/graph_msf_ros_examples/CMakeLists.txt (100%) rename {examples/ros => ros/examples}/graph_msf_ros_examples/package.xml (100%) rename {examples/ros => ros/examples}/imu_pose3_fuser/CMakeLists.txt (100%) rename {examples/ros => ros/examples}/imu_pose3_fuser/config/core/core_extrinsic_params.yaml (100%) rename {examples/ros => ros/examples}/imu_pose3_fuser/config/core/core_graph_config.yaml (100%) rename {examples/ros => ros/examples}/imu_pose3_fuser/config/core/core_graph_params.yaml (100%) rename {examples/ros => ros/examples}/imu_pose3_fuser/config/specific/imu_pose3_fuser_extrinsic_params.yaml (100%) rename {examples/ros => ros/examples}/imu_pose3_fuser/config/specific/imu_pose3_fuser_params.yaml (100%) rename {examples/ros => ros/examples}/imu_pose3_fuser/include/imu_pose3_fuser/ImuPose3Fuser.h (100%) rename {examples/ros => ros/examples}/imu_pose3_fuser/include/imu_pose3_fuser/constants.h (100%) rename {examples/ros => ros/examples}/imu_pose3_fuser/launch/imu_pose3_fuser.launch (100%) rename {examples/ros => ros/examples}/imu_pose3_fuser/launch/imu_pose3_fuser_replay.launch (100%) rename {examples/ros => ros/examples}/imu_pose3_fuser/package.xml (100%) rename {examples/ros => ros/examples}/imu_pose3_fuser/rviz/estimation.rviz (100%) rename {examples/ros => ros/examples}/imu_pose3_fuser/src/imu_pose3_fuser_node.cpp (100%) rename {examples/ros => ros/examples}/imu_pose3_fuser/src/lib/ImuPose3Fuser.cpp (100%) rename {examples/ros => ros/examples}/imu_pose3_fuser/src/lib/readParams.cpp (100%) rename {examples/ros => ros/examples}/pure_imu_integration/CMakeLists.txt (100%) rename {examples/ros => ros/examples}/pure_imu_integration/include/pure_imu_integration/ImuIntegrator.h (100%) rename {examples/ros => ros/examples}/pure_imu_integration/launch/dead_reckoning.launch (100%) rename {examples/ros => ros/examples}/pure_imu_integration/launch/dead_reckoning_replay.launch (100%) rename {examples/ros => ros/examples}/pure_imu_integration/package.xml (100%) rename {examples/ros => ros/examples}/pure_imu_integration/rviz/imu_integration.rviz (100%) rename {examples/ros => ros/examples}/pure_imu_integration/src/lib/ImuIntegrator.cpp (100%) rename {examples/ros => ros/examples}/pure_imu_integration/src/pure_imu_integration_node.cpp (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/CMakeLists.txt (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/FindGflags.cmake (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/FindGlog.cmake (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/config/compslam/compslam_lio_RS16_params.yaml (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/config/core/core_extrinsic_params.yaml (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/config/core/core_graph_config.yaml (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/config/core/core_graph_params.yaml (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/config/dumped_params/2023-06-13-13-15-57.yaml (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/config/smb_specific/smb_extrinsic_params.yaml (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/config/smb_specific/smb_graph_params.yaml (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/include/smb_estimator_graph/SmbEstimator.h (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/include/smb_estimator_graph/SmbStaticTransforms.h (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/include/smb_estimator_graph/constants.h (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/launch/smb_estimator_graph.launch (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/launch/smb_estimator_graph_replay.launch (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/package.xml (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/rviz/lidar_estimation.rviz (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/scripts/remove_map_and_odom_from_bag.py (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/src/lib/SmbEstimator.cpp (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/src/lib/SmbStaticTransforms.cpp (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/src/lib/readParams.cpp (100%) rename {examples/ros => ros/examples}/smb_estimator_graph/src/smb_estimator_node.cpp (100%) create mode 100644 ros2/examples/README.md diff --git a/examples/README.md b/examples/README.md deleted file mode 100644 index 5db8bfec..00000000 --- a/examples/README.md +++ /dev/null @@ -1,44 +0,0 @@ -# Examples - -## excavator_dual_graph - -### Visualization -For a visualization of a simplified model of our excavator HEAP clone the following repository into the workspace: -```bash -https://github.com/leggedrobotics/rsl_heap.git -``` -This step can also be left out. - -### Compiling the Code -This is the example of the code of our ICRA2022 paper [1]. -For compiling this code simply run: -```bash -catkin build excavator_dual_graph -``` - -### Data -For two example scenarios on our test-field at ETH Zurich please download the data located at the -[Google Drive Link](https://drive.google.com/drive/folders/1qZg_DNH3wXnQu4tNIcqY925KZFDu8y0M?usp=sharing). - -### Running the Code -In terminal 1 run (after sourcing the workspace): -```bash -roslaunch excavator_dual_graph dual_graph.launch -# or if heap_urdf not available -roslaunch excavator_dual_graph dual_graph_no_urdf.launch -``` -In terminal 2 play one of the rosbags located at the provided link location. Make sure to set the `--clock`-flag. -Either -```bash -rosbag play 1_digging_filtered.bag --clock -# or -rosbag play 2_driving_away_filtered.bag --clock -``` - -### Expected Output -The expected output looks like this: - - - -Here, yellow depicts the measured GNSS path, green depicts the estimated path in the world frame, and blue depicts the -estimated path in the odometry frame. \ No newline at end of file diff --git a/examples/_playground/gtsam_tryout_examples/CMakeLists.txt b/examples/_playground/gtsam_tryout_examples/CMakeLists.txt deleted file mode 100644 index 93253bce..00000000 --- a/examples/_playground/gtsam_tryout_examples/CMakeLists.txt +++ /dev/null @@ -1,108 +0,0 @@ -cmake_minimum_required(VERSION 3.16) -project(gtsam_tryout_examples) - -## Compile as C++17, supported in ROS Noetic and newer -add_compile_options(-std=c++17) - -# Find Dependencies --------------------------------------------------------------------------------------------------- -find_package(Eigen3 REQUIRED) -message("Eigen Version:" ${EIGEN3_VERSION_STRING}) -message("Eigen Path:" ${Eigen3_DIR}) - -# Color -if (NOT WIN32) - string(ASCII 27 Esc) - set(ColourReset "${Esc}[m") - set(BoldMagenta "${Esc}[1;35m") - set(Magenta "${Esc}[35m") -endif () - -# March native check -#include(CheckCXXCompilerFlag) -#CHECK_CXX_COMPILER_FLAG("-march=native" COMPILER_SUPPORTS_MARCH_NATIVE) -#if(COMPILER_SUPPORTS_MARCH_NATIVE) -# add_compile_options("-march=native") -# message("${BoldMagenta}INFO: Using -march=native${ColourReset}") -#endif() - -# Catkin dependencies ------------------------------------------------------------------------------------------------- -set(CATKIN_PACKAGE_DEPENDENCIES - gtsam_catkin - graph_msf_ros - nav_msgs - sensor_msgs - roscpp - std_msgs - tf -) - -find_package(catkin REQUIRED COMPONENTS - ${CATKIN_PACKAGE_DEPENDENCIES} -) - -catkin_package( - INCLUDE_DIRS - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS ${CATKIN_PACKAGE_DEPENDENCIES} -) - -########### -## Build ## -########### -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_STANDARD_REQUIRED ON) - -include_directories( - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} -) - -# Library -#add_library(${PROJECT_NAME} -# src/lib/SmbEstimator.cpp -# src/lib/readParams.cpp -# src/lib/SmbStaticTransforms.cpp -# include/smb_estimator_graph/constants.h) -#target_link_libraries(${PROJECT_NAME} -# ${catkin_LIBRARIES}) - -# 1: Alignment Executable -add_executable(${PROJECT_NAME}_alignment_node src/alignment_tryout_main.cpp) -target_link_libraries(${PROJECT_NAME}_alignment_node ${catkin_LIBRARIES}) - -# 2: Fixed Lag Smoother Invalid Key -add_executable(${PROJECT_NAME}_fixed_lag_smoother_invalid_key_node src/fixed_lag_smoother_invalid_key_main.cpp) -target_link_libraries(${PROJECT_NAME}_fixed_lag_smoother_invalid_key_node ${catkin_LIBRARIES}) - -#add_dependencies(${PROJECT_NAME} -# ${catkin_EXPORTED_TARGETS}) - -# Add clang tooling -find_package(cmake_clang_tools QUIET) -if (cmake_clang_tools_FOUND AND NOT DEFINED NO_CLANG_TOOLING) - add_clang_tooling( - TARGET ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include - CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include - CF_FIX - ) -endif (cmake_clang_tools_FOUND AND NOT DEFINED NO_CLANG_TOOLING) - -############# -## Install ## -############# - -#install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -#) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -## Mark cpp header files for installation -install(DIRECTORY config launch rviz - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/examples/_playground/gtsam_tryout_examples/package.xml b/examples/_playground/gtsam_tryout_examples/package.xml deleted file mode 100644 index 3c0d2000..00000000 --- a/examples/_playground/gtsam_tryout_examples/package.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - gtsam_tryout_examples - 0.0.0 - - - State estimation based on factor graphs, utilizing GTSAM functionality. Fusion of IMU, LiDAR Mapping Odometry and GNSS estimates. - - - Julian Nubert - BSD - Julian Nubert - - catkin - - gtsam_catkin - graph_msf_ros - nav_msgs - roscpp - std_msgs - sensor_msgs - tf - std_srvs - - gtsam_catkin - graph_msf_ros - nav_msgs - roscpp - std_msgs - sensor_msgs - tf - std_srvs - - rostest - rosbag - - diff --git a/examples/_playground/gtsam_tryout_examples/src/alignment_tryout_main.cpp b/examples/_playground/gtsam_tryout_examples/src/alignment_tryout_main.cpp deleted file mode 100644 index c6e58a0b..00000000 --- a/examples/_playground/gtsam_tryout_examples/src/alignment_tryout_main.cpp +++ /dev/null @@ -1,85 +0,0 @@ -/* -Copyright 2024 by Julian Nubert, Robotic Systems Lab, ETH Zurich. -All rights reserved. -This file is released under the "BSD-3-Clause License". -Please see the LICENSE file that has been included as part of this package. - */ - -// Gtsam -#include -#include -#include -#include -#include - -int main(int argc, char** argv) { - // Gtsam nonlinear graph - // gtsam::NonlinearFactorGraph graph; - gtsam::ExpressionFactorGraph graph; - - // Values - gtsam::Values initialValues; - - // Add prior on the first key - auto priorModel = gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); - gtsam::Key graphKey = 0; - - // Measurements - // GNSS in world frame - gtsam::Pose3 T_W_I_0 = gtsam::Pose3(gtsam::Rot3::RzRyRx(0.0, 0.0, 0.0), gtsam::Point3(0.0, 0.0, 0.0)); - gtsam::Pose3 T_W_I_1 = gtsam::Pose3(gtsam::Rot3::RzRyRx(0.0, 0.0, 0.0), gtsam::Point3(1.0, 0.0, 0.0)); - gtsam::Pose3 T_W_I_2 = gtsam::Pose3(gtsam::Rot3::RzRyRx(0.0, 0.0, 0.0), gtsam::Point3(2.0, 0.0, 0.0)); - gtsam::Pose3 T_M_W(gtsam::Rot3::RzRyRx(0.0, 0.0, M_PI / 2.0), gtsam::Point3(0.0, 0.0, 0.0)); - // LiDAR in map frame - gtsam::Pose3 T_M_I_0 = T_M_W * T_W_I_0; - gtsam::Pose3 T_M_I_1 = T_M_W * T_W_I_1; - gtsam::Pose3 T_M_I_2 = T_M_W * T_W_I_2; - - // Expressions - gtsam::Pose3_ T_M_W_(gtsam::symbol_shorthand::T(graphKey)); - gtsam::Pose3_ x0_(gtsam::symbol_shorthand::X(0)); - gtsam::Pose3_ x1_(gtsam::symbol_shorthand::X(graphKey + 1)); - gtsam::Pose3_ x2_(gtsam::symbol_shorthand::X(graphKey + 2)); - - // Add measurements and values - graph.addPrior(gtsam::symbol_shorthand::X(graphKey), T_W_I_0, priorModel); - graph.addExpressionFactor(gtsam::Pose3_(T_M_W_ * x0_), T_M_I_0, priorModel); - // graph.addPrior(gtsam::symbol_shorthand::X(graphKey), lidarPose0, priorModel); - initialValues.insert(gtsam::symbol_shorthand::X(graphKey), T_W_I_0); - initialValues.insert(gtsam::symbol_shorthand::T(graphKey), gtsam::Pose3()); - ++graphKey; - graph.addPrior(gtsam::symbol_shorthand::X(graphKey), T_W_I_1, priorModel); - graph.addExpressionFactor(gtsam::Pose3_(T_M_W_ * x1_), T_M_I_1, priorModel); - // graph.addPrior(gtsam::symbol_shorthand::X(graphKey), lidarPose1, priorModel); - initialValues.insert(gtsam::symbol_shorthand::X(graphKey), T_W_I_0); - ++graphKey; - gtsam::PriorFactor priorFactor(gtsam::symbol_shorthand::X(graphKey), T_W_I_2, priorModel); - graph.add(priorFactor); - gtsam::ExpressionFactor expressionFactor(priorModel, T_M_I_2, gtsam::Pose3_(T_M_W_ * x2_)); - graph.add(expressionFactor); - // graph.addPrior(gtsam::symbol_shorthand::X(graphKey), lidarPose2, priorModel); - initialValues.insert(gtsam::symbol_shorthand::X(graphKey), T_W_I_0); - - // Optimize - gtsam::LevenbergMarquardtParams params; - gtsam::LevenbergMarquardtOptimizer optimizer(graph, initialValues, params); - gtsam::Values result = optimizer.optimize(); - std::cout << "Optimization complete." << std::endl; - - // Conclude - std::cout << "initial error=" << graph.error(initialValues) << std::endl; - std::cout << "final error=" << graph.error(result) << std::endl; - - // Print result - std::cout << "----------------------------------------\n"; - result.print("Final result:\n"); - - // Print ground truth vs optimized map to world - std::cout << "----------------------------------------\n"; - std::cout << "Ground truth map to world:\n"; - std::cout << T_M_W.matrix() << std::endl; - std::cout << "Optimized map to world:\n"; - std::cout << result.at(gtsam::symbol_shorthand::T(0)).matrix() << std::endl; - - return 0; -} diff --git a/examples/_playground/gtsam_tryout_examples/src/fixed_lag_smoother_invalid_key_main.cpp b/examples/_playground/gtsam_tryout_examples/src/fixed_lag_smoother_invalid_key_main.cpp deleted file mode 100644 index 0d9b2156..00000000 --- a/examples/_playground/gtsam_tryout_examples/src/fixed_lag_smoother_invalid_key_main.cpp +++ /dev/null @@ -1,128 +0,0 @@ -/* -Copyright 2024 by Julian Nubert, Robotic Systems Lab, ETH Zurich. -All rights reserved. -This file is released under the "BSD-3-Clause License". -Please see the LICENSE file that has been included as part of this package. - */ - -// Gtsam -// 1. Generic Datatypes -// 1.1 Symbol Shorthand -#include -// 1.2 Pose3 -#include -// 2. Optimizers -// 2.1 Standard Batch -#include -#include "gtsam/nonlinear/ISAM2.h" -// 2.2 Incremental Fixed Lag Smoother -#include -// 3. Factors -// 3.1 IMU -#include -// 3.2 Between Factor -#include -// 3.3 Prior Factor -#include - -int main(int argc, char** argv) { - // Gtsam nonlinear graph - gtsam::NonlinearFactorGraph graph; - - // Values - gtsam::Values initialValues; - - // Noise Models - auto priorModel = gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); - auto betweenModel = gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); - - // Measurements - // GNSS in world frame - gtsam::Pose3 T_W_I0 = gtsam::Pose3(gtsam::Rot3::RzRyRx(0.0, 0.0, 0.0), gtsam::Point3(0.0, 0.0, 0.0)); - gtsam::Pose3 T_I0_I1 = gtsam::Pose3(gtsam::Rot3::RzRyRx(0.0, 0.0, 0.0), gtsam::Point3(1.0, 0.0, 0.0)); - gtsam::Pose3 T_I1_I2 = gtsam::Pose3(gtsam::Rot3::RzRyRx(0.0, 0.0, 0.0), gtsam::Point3(1.0, 0.0, 0.0)); - gtsam::Pose3 T_I2_I3 = gtsam::Pose3(gtsam::Rot3::RzRyRx(0.0, 0.0, 0.0), gtsam::Point3(1.0, 0.0, 0.0)); - gtsam::Pose3 T_I3_I4 = gtsam::Pose3(gtsam::Rot3::RzRyRx(0.0, 0.0, 0.0), gtsam::Point3(1.0, 0.0, 0.0)); - - // Add measurements and values - // Initial Pose - gtsam::Key graphKey = 0; - graph.addPrior(gtsam::symbol_shorthand::X(graphKey), T_W_I0, priorModel); - initialValues.insert(gtsam::symbol_shorthand::X(graphKey), T_W_I0); - - // Between Poses - // 0 -> 1 - graph.add(gtsam::BetweenFactor(gtsam::symbol_shorthand::X(graphKey), gtsam::symbol_shorthand::X(graphKey + 1), T_I0_I1, - betweenModel)); - // Delta noise in tangent space - gtsam::Pose3 deltaNoise = gtsam::Pose3::Expmap((gtsam::Vector(6) << 1e-3, 1e-3, 1e-3, 1e-2, 1e-2, 1e-2).finished()); - initialValues.insert(gtsam::symbol_shorthand::X(graphKey + 1), T_W_I0.compose(T_I0_I1).compose(deltaNoise)); - // 1 -> 2 - ++graphKey; - graph.add(gtsam::BetweenFactor(gtsam::symbol_shorthand::X(graphKey), gtsam::symbol_shorthand::X(graphKey + 1), T_I1_I2, - betweenModel)); - initialValues.insert(gtsam::symbol_shorthand::X(graphKey + 1), T_W_I0.compose(T_I0_I1.compose(T_I1_I2)).compose(deltaNoise)); - // 2 -> 3 - ++graphKey; - graph.add(gtsam::BetweenFactor(gtsam::symbol_shorthand::X(graphKey), gtsam::symbol_shorthand::X(graphKey + 1), T_I2_I3, - betweenModel)); - initialValues.insert(gtsam::symbol_shorthand::X(graphKey + 1), - T_W_I0.compose(T_I0_I1.compose(T_I1_I2.compose(T_I2_I3))).compose(deltaNoise)); - // 3 -> 4 - ++graphKey; - graph.add(gtsam::BetweenFactor(gtsam::symbol_shorthand::X(graphKey), gtsam::symbol_shorthand::X(graphKey + 1), T_I3_I4, - betweenModel)); - initialValues.insert(gtsam::symbol_shorthand::X(graphKey + 1), - T_W_I0.compose(T_I0_I1.compose(T_I1_I2.compose(T_I2_I3.compose(T_I3_I4)))).compose(deltaNoise)); - - // Optimize --> Classic - gtsam::LevenbergMarquardtParams params; - gtsam::LevenbergMarquardtOptimizer optimizer(graph, initialValues, params); - gtsam::Values lmResult = optimizer.optimize(); - std::cout << "Classic LM-Optimization complete." << std::endl; - - // Conclude - std::cout << "initial error=" << graph.error(initialValues) << std::endl; - std::cout << "final error=" << graph.error(lmResult) << std::endl; - - // Same with ISAM2 - gtsam::ISAM2Params isam2Params; - gtsam::ISAM2 isam2(isam2Params); - auto isam2Result = isam2.update(graph, initialValues); - std::cout << "ISAM2 Optimization complete." << std::endl; - - // Conclude - std::cout << "initial error=" << graph.error(initialValues) << std::endl; - - // Now with IncrementalFixedLagSmoother - // Case 1: All in smoother lag - gtsam::IncrementalFixedLagSmoother fixedLagSmoother1(3, isam2Params); - std::map allInGraphKeysTimeStampMap; - // 0->2s - for (size_t i = 0; i < 3; ++i) { - allInGraphKeysTimeStampMap.insert(std::make_pair(gtsam::symbol_shorthand::X(i), i)); - } - auto fixedLagResult = fixedLagSmoother1.update(graph, initialValues, allInGraphKeysTimeStampMap); - // 2->4s - for (size_t i = 3; i < 5; ++i) { - allInGraphKeysTimeStampMap.insert(std::make_pair(gtsam::symbol_shorthand::X(i), i)); - } - std::cout << "Step by step IncrementalFixedLagSmoother Optimization complete." << std::endl; - // Conclude - std::cout << "initial error=" << graph.error(initialValues) << std::endl; - - // Case 2: Not in smoother lag - gtsam::IncrementalFixedLagSmoother fixedLagSmoother2(3, isam2Params); - std::map notInGraphKeysTimeStampMap; - // 0->4s - for (size_t i = 0; i < 5; ++i) { - notInGraphKeysTimeStampMap.insert(std::make_pair(gtsam::symbol_shorthand::X(i), i)); - } - auto fixedLagResult2 = fixedLagSmoother2.update(graph, initialValues, notInGraphKeysTimeStampMap); - std::cout << "All at once IncrementalFixedLagSmoother Optimization complete." << std::endl; - // Conclude - std::cout << "initial error=" << graph.error(initialValues) << std::endl; - - // Return - return 0; -} diff --git a/examples/ros2/README.md b/examples/ros2/README.md deleted file mode 100644 index c92895de..00000000 --- a/examples/ros2/README.md +++ /dev/null @@ -1 +0,0 @@ -# ROS2 Examples diff --git a/examples/ros/anymal_estimator_graph/CMakeLists.txt b/ros/examples/anymal_estimator_graph/CMakeLists.txt similarity index 100% rename from examples/ros/anymal_estimator_graph/CMakeLists.txt rename to ros/examples/anymal_estimator_graph/CMakeLists.txt diff --git a/examples/ros/anymal_estimator_graph/FindGflags.cmake b/ros/examples/anymal_estimator_graph/FindGflags.cmake similarity index 100% rename from examples/ros/anymal_estimator_graph/FindGflags.cmake rename to ros/examples/anymal_estimator_graph/FindGflags.cmake diff --git a/examples/ros/anymal_estimator_graph/FindGlog.cmake b/ros/examples/anymal_estimator_graph/FindGlog.cmake similarity index 100% rename from examples/ros/anymal_estimator_graph/FindGlog.cmake rename to ros/examples/anymal_estimator_graph/FindGlog.cmake diff --git a/examples/ros/anymal_estimator_graph/README.md b/ros/examples/anymal_estimator_graph/README.md similarity index 100% rename from examples/ros/anymal_estimator_graph/README.md rename to ros/examples/anymal_estimator_graph/README.md diff --git a/examples/ros/anymal_estimator_graph/config/anymal_specific/anymal_extrinsic_params.yaml b/ros/examples/anymal_estimator_graph/config/anymal_specific/anymal_extrinsic_params.yaml similarity index 100% rename from examples/ros/anymal_estimator_graph/config/anymal_specific/anymal_extrinsic_params.yaml rename to ros/examples/anymal_estimator_graph/config/anymal_specific/anymal_extrinsic_params.yaml diff --git a/examples/ros/anymal_estimator_graph/config/anymal_specific/anymal_gnss_params.yaml b/ros/examples/anymal_estimator_graph/config/anymal_specific/anymal_gnss_params.yaml similarity index 100% rename from examples/ros/anymal_estimator_graph/config/anymal_specific/anymal_gnss_params.yaml rename to ros/examples/anymal_estimator_graph/config/anymal_specific/anymal_gnss_params.yaml diff --git a/examples/ros/anymal_estimator_graph/config/anymal_specific/anymal_graph_params_gnss.yaml b/ros/examples/anymal_estimator_graph/config/anymal_specific/anymal_graph_params_gnss.yaml similarity index 100% rename from examples/ros/anymal_estimator_graph/config/anymal_specific/anymal_graph_params_gnss.yaml rename to ros/examples/anymal_estimator_graph/config/anymal_specific/anymal_graph_params_gnss.yaml diff --git a/examples/ros/anymal_estimator_graph/config/anymal_specific/anymal_graph_params_no_gnss.yaml b/ros/examples/anymal_estimator_graph/config/anymal_specific/anymal_graph_params_no_gnss.yaml similarity index 100% rename from examples/ros/anymal_estimator_graph/config/anymal_specific/anymal_graph_params_no_gnss.yaml rename to ros/examples/anymal_estimator_graph/config/anymal_specific/anymal_graph_params_no_gnss.yaml diff --git a/examples/ros/anymal_estimator_graph/config/anymal_specific/anymal_traj_align_params.yaml b/ros/examples/anymal_estimator_graph/config/anymal_specific/anymal_traj_align_params.yaml similarity index 100% rename from examples/ros/anymal_estimator_graph/config/anymal_specific/anymal_traj_align_params.yaml rename to ros/examples/anymal_estimator_graph/config/anymal_specific/anymal_traj_align_params.yaml diff --git a/examples/ros/anymal_estimator_graph/config/core/core_extrinsic_params.yaml b/ros/examples/anymal_estimator_graph/config/core/core_extrinsic_params.yaml similarity index 100% rename from examples/ros/anymal_estimator_graph/config/core/core_extrinsic_params.yaml rename to ros/examples/anymal_estimator_graph/config/core/core_extrinsic_params.yaml diff --git a/examples/ros/anymal_estimator_graph/config/core/core_graph_config.yaml b/ros/examples/anymal_estimator_graph/config/core/core_graph_config.yaml similarity index 100% rename from examples/ros/anymal_estimator_graph/config/core/core_graph_config.yaml rename to ros/examples/anymal_estimator_graph/config/core/core_graph_config.yaml diff --git a/examples/ros/anymal_estimator_graph/config/core/core_graph_params_gnss.yaml b/ros/examples/anymal_estimator_graph/config/core/core_graph_params_gnss.yaml similarity index 100% rename from examples/ros/anymal_estimator_graph/config/core/core_graph_params_gnss.yaml rename to ros/examples/anymal_estimator_graph/config/core/core_graph_params_gnss.yaml diff --git a/examples/ros/anymal_estimator_graph/config/core/core_graph_params_no_gnss.yaml b/ros/examples/anymal_estimator_graph/config/core/core_graph_params_no_gnss.yaml similarity index 100% rename from examples/ros/anymal_estimator_graph/config/core/core_graph_params_no_gnss.yaml rename to ros/examples/anymal_estimator_graph/config/core/core_graph_params_no_gnss.yaml diff --git a/examples/ros/anymal_estimator_graph/config/dumped_params_d/2023-01-13-16-27-14_anymal-d020-lpc_mission.yaml b/ros/examples/anymal_estimator_graph/config/dumped_params_d/2023-01-13-16-27-14_anymal-d020-lpc_mission.yaml similarity index 100% rename from examples/ros/anymal_estimator_graph/config/dumped_params_d/2023-01-13-16-27-14_anymal-d020-lpc_mission.yaml rename to ros/examples/anymal_estimator_graph/config/dumped_params_d/2023-01-13-16-27-14_anymal-d020-lpc_mission.yaml diff --git a/examples/ros/anymal_estimator_graph/config/elevation_mapping/default.yaml b/ros/examples/anymal_estimator_graph/config/elevation_mapping/default.yaml similarity index 100% rename from examples/ros/anymal_estimator_graph/config/elevation_mapping/default.yaml rename to ros/examples/anymal_estimator_graph/config/elevation_mapping/default.yaml diff --git a/examples/ros/anymal_estimator_graph/config/elevation_mapping/offline_vlp16.yaml b/ros/examples/anymal_estimator_graph/config/elevation_mapping/offline_vlp16.yaml similarity index 100% rename from examples/ros/anymal_estimator_graph/config/elevation_mapping/offline_vlp16.yaml rename to ros/examples/anymal_estimator_graph/config/elevation_mapping/offline_vlp16.yaml diff --git a/examples/ros/anymal_estimator_graph/config/elevation_mapping/online_vlp16.yaml b/ros/examples/anymal_estimator_graph/config/elevation_mapping/online_vlp16.yaml similarity index 100% rename from examples/ros/anymal_estimator_graph/config/elevation_mapping/online_vlp16.yaml rename to ros/examples/anymal_estimator_graph/config/elevation_mapping/online_vlp16.yaml diff --git a/examples/ros/anymal_estimator_graph/include/anymal_estimator_graph/AnymalEstimator.h b/ros/examples/anymal_estimator_graph/include/anymal_estimator_graph/AnymalEstimator.h similarity index 100% rename from examples/ros/anymal_estimator_graph/include/anymal_estimator_graph/AnymalEstimator.h rename to ros/examples/anymal_estimator_graph/include/anymal_estimator_graph/AnymalEstimator.h diff --git a/examples/ros/anymal_estimator_graph/include/anymal_estimator_graph/AnymalStaticTransforms.h b/ros/examples/anymal_estimator_graph/include/anymal_estimator_graph/AnymalStaticTransforms.h similarity index 100% rename from examples/ros/anymal_estimator_graph/include/anymal_estimator_graph/AnymalStaticTransforms.h rename to ros/examples/anymal_estimator_graph/include/anymal_estimator_graph/AnymalStaticTransforms.h diff --git a/examples/ros/anymal_estimator_graph/include/anymal_estimator_graph/constants.h b/ros/examples/anymal_estimator_graph/include/anymal_estimator_graph/constants.h similarity index 100% rename from examples/ros/anymal_estimator_graph/include/anymal_estimator_graph/constants.h rename to ros/examples/anymal_estimator_graph/include/anymal_estimator_graph/constants.h diff --git a/examples/ros/anymal_estimator_graph/launch/anymal_estimator_graph.launch b/ros/examples/anymal_estimator_graph/launch/anymal_estimator_graph.launch similarity index 100% rename from examples/ros/anymal_estimator_graph/launch/anymal_estimator_graph.launch rename to ros/examples/anymal_estimator_graph/launch/anymal_estimator_graph.launch diff --git a/examples/ros/anymal_estimator_graph/launch/anymal_estimator_graph_replay.launch b/ros/examples/anymal_estimator_graph/launch/anymal_estimator_graph_replay.launch similarity index 100% rename from examples/ros/anymal_estimator_graph/launch/anymal_estimator_graph_replay.launch rename to ros/examples/anymal_estimator_graph/launch/anymal_estimator_graph_replay.launch diff --git a/examples/ros/anymal_estimator_graph/launch/anymal_estimator_graph_replay_offline_bag.launch b/ros/examples/anymal_estimator_graph/launch/anymal_estimator_graph_replay_offline_bag.launch similarity index 100% rename from examples/ros/anymal_estimator_graph/launch/anymal_estimator_graph_replay_offline_bag.launch rename to ros/examples/anymal_estimator_graph/launch/anymal_estimator_graph_replay_offline_bag.launch diff --git a/examples/ros/anymal_estimator_graph/launch/anymal_estimator_graph_replay_with_compslam.launch b/ros/examples/anymal_estimator_graph/launch/anymal_estimator_graph_replay_with_compslam.launch similarity index 100% rename from examples/ros/anymal_estimator_graph/launch/anymal_estimator_graph_replay_with_compslam.launch rename to ros/examples/anymal_estimator_graph/launch/anymal_estimator_graph_replay_with_compslam.launch diff --git a/examples/ros/anymal_estimator_graph/launch/anymal_estimator_graph_replay_with_o3d_slam.launch b/ros/examples/anymal_estimator_graph/launch/anymal_estimator_graph_replay_with_o3d_slam.launch similarity index 100% rename from examples/ros/anymal_estimator_graph/launch/anymal_estimator_graph_replay_with_o3d_slam.launch rename to ros/examples/anymal_estimator_graph/launch/anymal_estimator_graph_replay_with_o3d_slam.launch diff --git a/examples/ros/anymal_estimator_graph/package.xml b/ros/examples/anymal_estimator_graph/package.xml similarity index 100% rename from examples/ros/anymal_estimator_graph/package.xml rename to ros/examples/anymal_estimator_graph/package.xml diff --git a/examples/ros/anymal_estimator_graph/rviz/gnss_lidar.rviz b/ros/examples/anymal_estimator_graph/rviz/gnss_lidar.rviz similarity index 100% rename from examples/ros/anymal_estimator_graph/rviz/gnss_lidar.rviz rename to ros/examples/anymal_estimator_graph/rviz/gnss_lidar.rviz diff --git a/examples/ros/anymal_estimator_graph/rviz/gnss_lidar_offline.rviz b/ros/examples/anymal_estimator_graph/rviz/gnss_lidar_offline.rviz similarity index 100% rename from examples/ros/anymal_estimator_graph/rviz/gnss_lidar_offline.rviz rename to ros/examples/anymal_estimator_graph/rviz/gnss_lidar_offline.rviz diff --git a/examples/ros/anymal_estimator_graph/src/anymal_estimator_node.cpp b/ros/examples/anymal_estimator_graph/src/anymal_estimator_node.cpp similarity index 100% rename from examples/ros/anymal_estimator_graph/src/anymal_estimator_node.cpp rename to ros/examples/anymal_estimator_graph/src/anymal_estimator_node.cpp diff --git a/examples/ros/anymal_estimator_graph/src/lib/AnymalEstimator.cpp b/ros/examples/anymal_estimator_graph/src/lib/AnymalEstimator.cpp similarity index 100% rename from examples/ros/anymal_estimator_graph/src/lib/AnymalEstimator.cpp rename to ros/examples/anymal_estimator_graph/src/lib/AnymalEstimator.cpp diff --git a/examples/ros/anymal_estimator_graph/src/lib/AnymalStaticTransforms.cpp b/ros/examples/anymal_estimator_graph/src/lib/AnymalStaticTransforms.cpp similarity index 100% rename from examples/ros/anymal_estimator_graph/src/lib/AnymalStaticTransforms.cpp rename to ros/examples/anymal_estimator_graph/src/lib/AnymalStaticTransforms.cpp diff --git a/examples/ros/anymal_estimator_graph/src/lib/readParams.cpp b/ros/examples/anymal_estimator_graph/src/lib/readParams.cpp similarity index 100% rename from examples/ros/anymal_estimator_graph/src/lib/readParams.cpp rename to ros/examples/anymal_estimator_graph/src/lib/readParams.cpp diff --git a/examples/ros/atn_position3_fuser/CMakeLists.txt b/ros/examples/atn_position3_fuser/CMakeLists.txt similarity index 100% rename from examples/ros/atn_position3_fuser/CMakeLists.txt rename to ros/examples/atn_position3_fuser/CMakeLists.txt diff --git a/examples/ros/atn_position3_fuser/FindGflags.cmake b/ros/examples/atn_position3_fuser/FindGflags.cmake similarity index 100% rename from examples/ros/atn_position3_fuser/FindGflags.cmake rename to ros/examples/atn_position3_fuser/FindGflags.cmake diff --git a/examples/ros/atn_position3_fuser/FindGlog.cmake b/ros/examples/atn_position3_fuser/FindGlog.cmake similarity index 100% rename from examples/ros/atn_position3_fuser/FindGlog.cmake rename to ros/examples/atn_position3_fuser/FindGlog.cmake diff --git a/examples/ros/atn_position3_fuser/README.md b/ros/examples/atn_position3_fuser/README.md similarity index 100% rename from examples/ros/atn_position3_fuser/README.md rename to ros/examples/atn_position3_fuser/README.md diff --git a/examples/ros/atn_position3_fuser/config/core/core_extrinsic_params.yaml b/ros/examples/atn_position3_fuser/config/core/core_extrinsic_params.yaml similarity index 100% rename from examples/ros/atn_position3_fuser/config/core/core_extrinsic_params.yaml rename to ros/examples/atn_position3_fuser/config/core/core_extrinsic_params.yaml diff --git a/examples/ros/atn_position3_fuser/config/core/core_graph_config.yaml b/ros/examples/atn_position3_fuser/config/core/core_graph_config.yaml similarity index 100% rename from examples/ros/atn_position3_fuser/config/core/core_graph_config.yaml rename to ros/examples/atn_position3_fuser/config/core/core_graph_config.yaml diff --git a/examples/ros/atn_position3_fuser/config/core/core_graph_params.yaml b/ros/examples/atn_position3_fuser/config/core/core_graph_params.yaml similarity index 100% rename from examples/ros/atn_position3_fuser/config/core/core_graph_params.yaml rename to ros/examples/atn_position3_fuser/config/core/core_graph_params.yaml diff --git a/examples/ros/atn_position3_fuser/config/elevation_mapping/default.yaml b/ros/examples/atn_position3_fuser/config/elevation_mapping/default.yaml similarity index 100% rename from examples/ros/atn_position3_fuser/config/elevation_mapping/default.yaml rename to ros/examples/atn_position3_fuser/config/elevation_mapping/default.yaml diff --git a/examples/ros/atn_position3_fuser/config/elevation_mapping/offline_vlp16.yaml b/ros/examples/atn_position3_fuser/config/elevation_mapping/offline_vlp16.yaml similarity index 100% rename from examples/ros/atn_position3_fuser/config/elevation_mapping/offline_vlp16.yaml rename to ros/examples/atn_position3_fuser/config/elevation_mapping/offline_vlp16.yaml diff --git a/examples/ros/atn_position3_fuser/config/elevation_mapping/online_vlp16.yaml b/ros/examples/atn_position3_fuser/config/elevation_mapping/online_vlp16.yaml similarity index 100% rename from examples/ros/atn_position3_fuser/config/elevation_mapping/online_vlp16.yaml rename to ros/examples/atn_position3_fuser/config/elevation_mapping/online_vlp16.yaml diff --git a/examples/ros/atn_position3_fuser/config/position_graph_specific/position_graph_extrinsic_params.yaml b/ros/examples/atn_position3_fuser/config/position_graph_specific/position_graph_extrinsic_params.yaml similarity index 100% rename from examples/ros/atn_position3_fuser/config/position_graph_specific/position_graph_extrinsic_params.yaml rename to ros/examples/atn_position3_fuser/config/position_graph_specific/position_graph_extrinsic_params.yaml diff --git a/examples/ros/atn_position3_fuser/config/position_graph_specific/position_graph_graph_params.yaml b/ros/examples/atn_position3_fuser/config/position_graph_specific/position_graph_graph_params.yaml similarity index 100% rename from examples/ros/atn_position3_fuser/config/position_graph_specific/position_graph_graph_params.yaml rename to ros/examples/atn_position3_fuser/config/position_graph_specific/position_graph_graph_params.yaml diff --git a/examples/ros/atn_position3_fuser/config/position_graph_specific/trajectory_alignment_params.yaml b/ros/examples/atn_position3_fuser/config/position_graph_specific/trajectory_alignment_params.yaml similarity index 100% rename from examples/ros/atn_position3_fuser/config/position_graph_specific/trajectory_alignment_params.yaml rename to ros/examples/atn_position3_fuser/config/position_graph_specific/trajectory_alignment_params.yaml diff --git a/examples/ros/atn_position3_fuser/include/atn_position3_fuser/Position3Estimator.h b/ros/examples/atn_position3_fuser/include/atn_position3_fuser/Position3Estimator.h similarity index 100% rename from examples/ros/atn_position3_fuser/include/atn_position3_fuser/Position3Estimator.h rename to ros/examples/atn_position3_fuser/include/atn_position3_fuser/Position3Estimator.h diff --git a/examples/ros/atn_position3_fuser/include/atn_position3_fuser/Position3StaticTransforms.h b/ros/examples/atn_position3_fuser/include/atn_position3_fuser/Position3StaticTransforms.h similarity index 100% rename from examples/ros/atn_position3_fuser/include/atn_position3_fuser/Position3StaticTransforms.h rename to ros/examples/atn_position3_fuser/include/atn_position3_fuser/Position3StaticTransforms.h diff --git a/examples/ros/atn_position3_fuser/include/atn_position3_fuser/constants.h b/ros/examples/atn_position3_fuser/include/atn_position3_fuser/constants.h similarity index 100% rename from examples/ros/atn_position3_fuser/include/atn_position3_fuser/constants.h rename to ros/examples/atn_position3_fuser/include/atn_position3_fuser/constants.h diff --git a/examples/ros/atn_position3_fuser/launch/position3_fuser.launch b/ros/examples/atn_position3_fuser/launch/position3_fuser.launch similarity index 100% rename from examples/ros/atn_position3_fuser/launch/position3_fuser.launch rename to ros/examples/atn_position3_fuser/launch/position3_fuser.launch diff --git a/examples/ros/atn_position3_fuser/launch/position3_fuser_replay.launch b/ros/examples/atn_position3_fuser/launch/position3_fuser_replay.launch similarity index 100% rename from examples/ros/atn_position3_fuser/launch/position3_fuser_replay.launch rename to ros/examples/atn_position3_fuser/launch/position3_fuser_replay.launch diff --git a/examples/ros/atn_position3_fuser/launch/position3_fuser_replay_offline_bag.launch b/ros/examples/atn_position3_fuser/launch/position3_fuser_replay_offline_bag.launch similarity index 100% rename from examples/ros/atn_position3_fuser/launch/position3_fuser_replay_offline_bag.launch rename to ros/examples/atn_position3_fuser/launch/position3_fuser_replay_offline_bag.launch diff --git a/examples/ros/atn_position3_fuser/package.xml b/ros/examples/atn_position3_fuser/package.xml similarity index 100% rename from examples/ros/atn_position3_fuser/package.xml rename to ros/examples/atn_position3_fuser/package.xml diff --git a/examples/ros/atn_position3_fuser/position3_fuser_py/check_gnss_in_bag.py b/ros/examples/atn_position3_fuser/position3_fuser_py/check_gnss_in_bag.py similarity index 100% rename from examples/ros/atn_position3_fuser/position3_fuser_py/check_gnss_in_bag.py rename to ros/examples/atn_position3_fuser/position3_fuser_py/check_gnss_in_bag.py diff --git a/examples/ros/atn_position3_fuser/rviz/rviz_imuOdom.rviz b/ros/examples/atn_position3_fuser/rviz/rviz_imuOdom.rviz similarity index 100% rename from examples/ros/atn_position3_fuser/rviz/rviz_imuOdom.rviz rename to ros/examples/atn_position3_fuser/rviz/rviz_imuOdom.rviz diff --git a/examples/ros/atn_position3_fuser/rviz/rviz_imuOdom_offline.rviz b/ros/examples/atn_position3_fuser/rviz/rviz_imuOdom_offline.rviz similarity index 100% rename from examples/ros/atn_position3_fuser/rviz/rviz_imuOdom_offline.rviz rename to ros/examples/atn_position3_fuser/rviz/rviz_imuOdom_offline.rviz diff --git a/examples/ros/atn_position3_fuser/setup.py b/ros/examples/atn_position3_fuser/setup.py similarity index 100% rename from examples/ros/atn_position3_fuser/setup.py rename to ros/examples/atn_position3_fuser/setup.py diff --git a/examples/ros/atn_position3_fuser/src/graph_node.cpp b/ros/examples/atn_position3_fuser/src/graph_node.cpp similarity index 100% rename from examples/ros/atn_position3_fuser/src/graph_node.cpp rename to ros/examples/atn_position3_fuser/src/graph_node.cpp diff --git a/examples/ros/atn_position3_fuser/src/lib/Position3Estimator.cpp b/ros/examples/atn_position3_fuser/src/lib/Position3Estimator.cpp similarity index 100% rename from examples/ros/atn_position3_fuser/src/lib/Position3Estimator.cpp rename to ros/examples/atn_position3_fuser/src/lib/Position3Estimator.cpp diff --git a/examples/ros/atn_position3_fuser/src/lib/Position3StaticTransforms.cpp b/ros/examples/atn_position3_fuser/src/lib/Position3StaticTransforms.cpp similarity index 100% rename from examples/ros/atn_position3_fuser/src/lib/Position3StaticTransforms.cpp rename to ros/examples/atn_position3_fuser/src/lib/Position3StaticTransforms.cpp diff --git a/examples/ros/atn_position3_fuser/src/lib/readParams.cpp b/ros/examples/atn_position3_fuser/src/lib/readParams.cpp similarity index 100% rename from examples/ros/atn_position3_fuser/src/lib/readParams.cpp rename to ros/examples/atn_position3_fuser/src/lib/readParams.cpp diff --git a/examples/ros/excavator_holistic_graph/CMakeLists.txt b/ros/examples/excavator_holistic_graph/CMakeLists.txt similarity index 100% rename from examples/ros/excavator_holistic_graph/CMakeLists.txt rename to ros/examples/excavator_holistic_graph/CMakeLists.txt diff --git a/examples/ros/excavator_holistic_graph/config/core/core_extrinsic_params.yaml b/ros/examples/excavator_holistic_graph/config/core/core_extrinsic_params.yaml similarity index 100% rename from examples/ros/excavator_holistic_graph/config/core/core_extrinsic_params.yaml rename to ros/examples/excavator_holistic_graph/config/core/core_extrinsic_params.yaml diff --git a/examples/ros/excavator_holistic_graph/config/core/core_graph_config.yaml b/ros/examples/excavator_holistic_graph/config/core/core_graph_config.yaml similarity index 100% rename from examples/ros/excavator_holistic_graph/config/core/core_graph_config.yaml rename to ros/examples/excavator_holistic_graph/config/core/core_graph_config.yaml diff --git a/examples/ros/excavator_holistic_graph/config/core/core_graph_params.yaml b/ros/examples/excavator_holistic_graph/config/core/core_graph_params.yaml similarity index 100% rename from examples/ros/excavator_holistic_graph/config/core/core_graph_params.yaml rename to ros/examples/excavator_holistic_graph/config/core/core_graph_params.yaml diff --git a/examples/ros/excavator_holistic_graph/config/excavator_specific/excavator_extrinsic_params.yaml b/ros/examples/excavator_holistic_graph/config/excavator_specific/excavator_extrinsic_params.yaml similarity index 100% rename from examples/ros/excavator_holistic_graph/config/excavator_specific/excavator_extrinsic_params.yaml rename to ros/examples/excavator_holistic_graph/config/excavator_specific/excavator_extrinsic_params.yaml diff --git a/examples/ros/excavator_holistic_graph/config/excavator_specific/excavator_gnss_params.yaml b/ros/examples/excavator_holistic_graph/config/excavator_specific/excavator_gnss_params.yaml similarity index 100% rename from examples/ros/excavator_holistic_graph/config/excavator_specific/excavator_gnss_params.yaml rename to ros/examples/excavator_holistic_graph/config/excavator_specific/excavator_gnss_params.yaml diff --git a/examples/ros/excavator_holistic_graph/config/excavator_specific/excavator_graph_params.yaml b/ros/examples/excavator_holistic_graph/config/excavator_specific/excavator_graph_params.yaml similarity index 100% rename from examples/ros/excavator_holistic_graph/config/excavator_specific/excavator_graph_params.yaml rename to ros/examples/excavator_holistic_graph/config/excavator_specific/excavator_graph_params.yaml diff --git a/examples/ros/excavator_holistic_graph/include/excavator_holistic_graph/ExcavatorEstimator.h b/ros/examples/excavator_holistic_graph/include/excavator_holistic_graph/ExcavatorEstimator.h similarity index 100% rename from examples/ros/excavator_holistic_graph/include/excavator_holistic_graph/ExcavatorEstimator.h rename to ros/examples/excavator_holistic_graph/include/excavator_holistic_graph/ExcavatorEstimator.h diff --git a/examples/ros/excavator_holistic_graph/include/excavator_holistic_graph/ExcavatorStaticTransforms.h b/ros/examples/excavator_holistic_graph/include/excavator_holistic_graph/ExcavatorStaticTransforms.h similarity index 100% rename from examples/ros/excavator_holistic_graph/include/excavator_holistic_graph/ExcavatorStaticTransforms.h rename to ros/examples/excavator_holistic_graph/include/excavator_holistic_graph/ExcavatorStaticTransforms.h diff --git a/examples/ros/excavator_holistic_graph/include/excavator_holistic_graph/constants.h b/ros/examples/excavator_holistic_graph/include/excavator_holistic_graph/constants.h similarity index 100% rename from examples/ros/excavator_holistic_graph/include/excavator_holistic_graph/constants.h rename to ros/examples/excavator_holistic_graph/include/excavator_holistic_graph/constants.h diff --git a/examples/ros/excavator_holistic_graph/launch/graph.launch b/ros/examples/excavator_holistic_graph/launch/graph.launch similarity index 100% rename from examples/ros/excavator_holistic_graph/launch/graph.launch rename to ros/examples/excavator_holistic_graph/launch/graph.launch diff --git a/examples/ros/excavator_holistic_graph/launch/graph_no_urdf.launch b/ros/examples/excavator_holistic_graph/launch/graph_no_urdf.launch similarity index 100% rename from examples/ros/excavator_holistic_graph/launch/graph_no_urdf.launch rename to ros/examples/excavator_holistic_graph/launch/graph_no_urdf.launch diff --git a/examples/ros/excavator_holistic_graph/package.xml b/ros/examples/excavator_holistic_graph/package.xml similarity index 100% rename from examples/ros/excavator_holistic_graph/package.xml rename to ros/examples/excavator_holistic_graph/package.xml diff --git a/examples/ros/excavator_holistic_graph/rviz/gnss_lidar.rviz b/ros/examples/excavator_holistic_graph/rviz/gnss_lidar.rviz similarity index 100% rename from examples/ros/excavator_holistic_graph/rviz/gnss_lidar.rviz rename to ros/examples/excavator_holistic_graph/rviz/gnss_lidar.rviz diff --git a/examples/ros/excavator_holistic_graph/scripts/remove_map_and_odom_from_bag.py b/ros/examples/excavator_holistic_graph/scripts/remove_map_and_odom_from_bag.py similarity index 100% rename from examples/ros/excavator_holistic_graph/scripts/remove_map_and_odom_from_bag.py rename to ros/examples/excavator_holistic_graph/scripts/remove_map_and_odom_from_bag.py diff --git a/examples/ros/excavator_holistic_graph/src/graph_node.cpp b/ros/examples/excavator_holistic_graph/src/graph_node.cpp similarity index 100% rename from examples/ros/excavator_holistic_graph/src/graph_node.cpp rename to ros/examples/excavator_holistic_graph/src/graph_node.cpp diff --git a/examples/ros/excavator_holistic_graph/src/lib/ExcavatorEstimator.cpp b/ros/examples/excavator_holistic_graph/src/lib/ExcavatorEstimator.cpp similarity index 100% rename from examples/ros/excavator_holistic_graph/src/lib/ExcavatorEstimator.cpp rename to ros/examples/excavator_holistic_graph/src/lib/ExcavatorEstimator.cpp diff --git a/examples/ros/excavator_holistic_graph/src/lib/ExcavatorStaticTransforms.cpp b/ros/examples/excavator_holistic_graph/src/lib/ExcavatorStaticTransforms.cpp similarity index 100% rename from examples/ros/excavator_holistic_graph/src/lib/ExcavatorStaticTransforms.cpp rename to ros/examples/excavator_holistic_graph/src/lib/ExcavatorStaticTransforms.cpp diff --git a/examples/ros/excavator_holistic_graph/src/lib/readParams.cpp b/ros/examples/excavator_holistic_graph/src/lib/readParams.cpp similarity index 100% rename from examples/ros/excavator_holistic_graph/src/lib/readParams.cpp rename to ros/examples/excavator_holistic_graph/src/lib/readParams.cpp diff --git a/examples/ros/graph_msf_ros_examples/CMakeLists.txt b/ros/examples/graph_msf_ros_examples/CMakeLists.txt similarity index 100% rename from examples/ros/graph_msf_ros_examples/CMakeLists.txt rename to ros/examples/graph_msf_ros_examples/CMakeLists.txt diff --git a/examples/ros/graph_msf_ros_examples/package.xml b/ros/examples/graph_msf_ros_examples/package.xml similarity index 100% rename from examples/ros/graph_msf_ros_examples/package.xml rename to ros/examples/graph_msf_ros_examples/package.xml diff --git a/examples/ros/imu_pose3_fuser/CMakeLists.txt b/ros/examples/imu_pose3_fuser/CMakeLists.txt similarity index 100% rename from examples/ros/imu_pose3_fuser/CMakeLists.txt rename to ros/examples/imu_pose3_fuser/CMakeLists.txt diff --git a/examples/ros/imu_pose3_fuser/config/core/core_extrinsic_params.yaml b/ros/examples/imu_pose3_fuser/config/core/core_extrinsic_params.yaml similarity index 100% rename from examples/ros/imu_pose3_fuser/config/core/core_extrinsic_params.yaml rename to ros/examples/imu_pose3_fuser/config/core/core_extrinsic_params.yaml diff --git a/examples/ros/imu_pose3_fuser/config/core/core_graph_config.yaml b/ros/examples/imu_pose3_fuser/config/core/core_graph_config.yaml similarity index 100% rename from examples/ros/imu_pose3_fuser/config/core/core_graph_config.yaml rename to ros/examples/imu_pose3_fuser/config/core/core_graph_config.yaml diff --git a/examples/ros/imu_pose3_fuser/config/core/core_graph_params.yaml b/ros/examples/imu_pose3_fuser/config/core/core_graph_params.yaml similarity index 100% rename from examples/ros/imu_pose3_fuser/config/core/core_graph_params.yaml rename to ros/examples/imu_pose3_fuser/config/core/core_graph_params.yaml diff --git a/examples/ros/imu_pose3_fuser/config/specific/imu_pose3_fuser_extrinsic_params.yaml b/ros/examples/imu_pose3_fuser/config/specific/imu_pose3_fuser_extrinsic_params.yaml similarity index 100% rename from examples/ros/imu_pose3_fuser/config/specific/imu_pose3_fuser_extrinsic_params.yaml rename to ros/examples/imu_pose3_fuser/config/specific/imu_pose3_fuser_extrinsic_params.yaml diff --git a/examples/ros/imu_pose3_fuser/config/specific/imu_pose3_fuser_params.yaml b/ros/examples/imu_pose3_fuser/config/specific/imu_pose3_fuser_params.yaml similarity index 100% rename from examples/ros/imu_pose3_fuser/config/specific/imu_pose3_fuser_params.yaml rename to ros/examples/imu_pose3_fuser/config/specific/imu_pose3_fuser_params.yaml diff --git a/examples/ros/imu_pose3_fuser/include/imu_pose3_fuser/ImuPose3Fuser.h b/ros/examples/imu_pose3_fuser/include/imu_pose3_fuser/ImuPose3Fuser.h similarity index 100% rename from examples/ros/imu_pose3_fuser/include/imu_pose3_fuser/ImuPose3Fuser.h rename to ros/examples/imu_pose3_fuser/include/imu_pose3_fuser/ImuPose3Fuser.h diff --git a/examples/ros/imu_pose3_fuser/include/imu_pose3_fuser/constants.h b/ros/examples/imu_pose3_fuser/include/imu_pose3_fuser/constants.h similarity index 100% rename from examples/ros/imu_pose3_fuser/include/imu_pose3_fuser/constants.h rename to ros/examples/imu_pose3_fuser/include/imu_pose3_fuser/constants.h diff --git a/examples/ros/imu_pose3_fuser/launch/imu_pose3_fuser.launch b/ros/examples/imu_pose3_fuser/launch/imu_pose3_fuser.launch similarity index 100% rename from examples/ros/imu_pose3_fuser/launch/imu_pose3_fuser.launch rename to ros/examples/imu_pose3_fuser/launch/imu_pose3_fuser.launch diff --git a/examples/ros/imu_pose3_fuser/launch/imu_pose3_fuser_replay.launch b/ros/examples/imu_pose3_fuser/launch/imu_pose3_fuser_replay.launch similarity index 100% rename from examples/ros/imu_pose3_fuser/launch/imu_pose3_fuser_replay.launch rename to ros/examples/imu_pose3_fuser/launch/imu_pose3_fuser_replay.launch diff --git a/examples/ros/imu_pose3_fuser/package.xml b/ros/examples/imu_pose3_fuser/package.xml similarity index 100% rename from examples/ros/imu_pose3_fuser/package.xml rename to ros/examples/imu_pose3_fuser/package.xml diff --git a/examples/ros/imu_pose3_fuser/rviz/estimation.rviz b/ros/examples/imu_pose3_fuser/rviz/estimation.rviz similarity index 100% rename from examples/ros/imu_pose3_fuser/rviz/estimation.rviz rename to ros/examples/imu_pose3_fuser/rviz/estimation.rviz diff --git a/examples/ros/imu_pose3_fuser/src/imu_pose3_fuser_node.cpp b/ros/examples/imu_pose3_fuser/src/imu_pose3_fuser_node.cpp similarity index 100% rename from examples/ros/imu_pose3_fuser/src/imu_pose3_fuser_node.cpp rename to ros/examples/imu_pose3_fuser/src/imu_pose3_fuser_node.cpp diff --git a/examples/ros/imu_pose3_fuser/src/lib/ImuPose3Fuser.cpp b/ros/examples/imu_pose3_fuser/src/lib/ImuPose3Fuser.cpp similarity index 100% rename from examples/ros/imu_pose3_fuser/src/lib/ImuPose3Fuser.cpp rename to ros/examples/imu_pose3_fuser/src/lib/ImuPose3Fuser.cpp diff --git a/examples/ros/imu_pose3_fuser/src/lib/readParams.cpp b/ros/examples/imu_pose3_fuser/src/lib/readParams.cpp similarity index 100% rename from examples/ros/imu_pose3_fuser/src/lib/readParams.cpp rename to ros/examples/imu_pose3_fuser/src/lib/readParams.cpp diff --git a/examples/ros/pure_imu_integration/CMakeLists.txt b/ros/examples/pure_imu_integration/CMakeLists.txt similarity index 100% rename from examples/ros/pure_imu_integration/CMakeLists.txt rename to ros/examples/pure_imu_integration/CMakeLists.txt diff --git a/examples/ros/pure_imu_integration/include/pure_imu_integration/ImuIntegrator.h b/ros/examples/pure_imu_integration/include/pure_imu_integration/ImuIntegrator.h similarity index 100% rename from examples/ros/pure_imu_integration/include/pure_imu_integration/ImuIntegrator.h rename to ros/examples/pure_imu_integration/include/pure_imu_integration/ImuIntegrator.h diff --git a/examples/ros/pure_imu_integration/launch/dead_reckoning.launch b/ros/examples/pure_imu_integration/launch/dead_reckoning.launch similarity index 100% rename from examples/ros/pure_imu_integration/launch/dead_reckoning.launch rename to ros/examples/pure_imu_integration/launch/dead_reckoning.launch diff --git a/examples/ros/pure_imu_integration/launch/dead_reckoning_replay.launch b/ros/examples/pure_imu_integration/launch/dead_reckoning_replay.launch similarity index 100% rename from examples/ros/pure_imu_integration/launch/dead_reckoning_replay.launch rename to ros/examples/pure_imu_integration/launch/dead_reckoning_replay.launch diff --git a/examples/ros/pure_imu_integration/package.xml b/ros/examples/pure_imu_integration/package.xml similarity index 100% rename from examples/ros/pure_imu_integration/package.xml rename to ros/examples/pure_imu_integration/package.xml diff --git a/examples/ros/pure_imu_integration/rviz/imu_integration.rviz b/ros/examples/pure_imu_integration/rviz/imu_integration.rviz similarity index 100% rename from examples/ros/pure_imu_integration/rviz/imu_integration.rviz rename to ros/examples/pure_imu_integration/rviz/imu_integration.rviz diff --git a/examples/ros/pure_imu_integration/src/lib/ImuIntegrator.cpp b/ros/examples/pure_imu_integration/src/lib/ImuIntegrator.cpp similarity index 100% rename from examples/ros/pure_imu_integration/src/lib/ImuIntegrator.cpp rename to ros/examples/pure_imu_integration/src/lib/ImuIntegrator.cpp diff --git a/examples/ros/pure_imu_integration/src/pure_imu_integration_node.cpp b/ros/examples/pure_imu_integration/src/pure_imu_integration_node.cpp similarity index 100% rename from examples/ros/pure_imu_integration/src/pure_imu_integration_node.cpp rename to ros/examples/pure_imu_integration/src/pure_imu_integration_node.cpp diff --git a/examples/ros/smb_estimator_graph/CMakeLists.txt b/ros/examples/smb_estimator_graph/CMakeLists.txt similarity index 100% rename from examples/ros/smb_estimator_graph/CMakeLists.txt rename to ros/examples/smb_estimator_graph/CMakeLists.txt diff --git a/examples/ros/smb_estimator_graph/FindGflags.cmake b/ros/examples/smb_estimator_graph/FindGflags.cmake similarity index 100% rename from examples/ros/smb_estimator_graph/FindGflags.cmake rename to ros/examples/smb_estimator_graph/FindGflags.cmake diff --git a/examples/ros/smb_estimator_graph/FindGlog.cmake b/ros/examples/smb_estimator_graph/FindGlog.cmake similarity index 100% rename from examples/ros/smb_estimator_graph/FindGlog.cmake rename to ros/examples/smb_estimator_graph/FindGlog.cmake diff --git a/examples/ros/smb_estimator_graph/config/compslam/compslam_lio_RS16_params.yaml b/ros/examples/smb_estimator_graph/config/compslam/compslam_lio_RS16_params.yaml similarity index 100% rename from examples/ros/smb_estimator_graph/config/compslam/compslam_lio_RS16_params.yaml rename to ros/examples/smb_estimator_graph/config/compslam/compslam_lio_RS16_params.yaml diff --git a/examples/ros/smb_estimator_graph/config/core/core_extrinsic_params.yaml b/ros/examples/smb_estimator_graph/config/core/core_extrinsic_params.yaml similarity index 100% rename from examples/ros/smb_estimator_graph/config/core/core_extrinsic_params.yaml rename to ros/examples/smb_estimator_graph/config/core/core_extrinsic_params.yaml diff --git a/examples/ros/smb_estimator_graph/config/core/core_graph_config.yaml b/ros/examples/smb_estimator_graph/config/core/core_graph_config.yaml similarity index 100% rename from examples/ros/smb_estimator_graph/config/core/core_graph_config.yaml rename to ros/examples/smb_estimator_graph/config/core/core_graph_config.yaml diff --git a/examples/ros/smb_estimator_graph/config/core/core_graph_params.yaml b/ros/examples/smb_estimator_graph/config/core/core_graph_params.yaml similarity index 100% rename from examples/ros/smb_estimator_graph/config/core/core_graph_params.yaml rename to ros/examples/smb_estimator_graph/config/core/core_graph_params.yaml diff --git a/examples/ros/smb_estimator_graph/config/dumped_params/2023-06-13-13-15-57.yaml b/ros/examples/smb_estimator_graph/config/dumped_params/2023-06-13-13-15-57.yaml similarity index 100% rename from examples/ros/smb_estimator_graph/config/dumped_params/2023-06-13-13-15-57.yaml rename to ros/examples/smb_estimator_graph/config/dumped_params/2023-06-13-13-15-57.yaml diff --git a/examples/ros/smb_estimator_graph/config/smb_specific/smb_extrinsic_params.yaml b/ros/examples/smb_estimator_graph/config/smb_specific/smb_extrinsic_params.yaml similarity index 100% rename from examples/ros/smb_estimator_graph/config/smb_specific/smb_extrinsic_params.yaml rename to ros/examples/smb_estimator_graph/config/smb_specific/smb_extrinsic_params.yaml diff --git a/examples/ros/smb_estimator_graph/config/smb_specific/smb_graph_params.yaml b/ros/examples/smb_estimator_graph/config/smb_specific/smb_graph_params.yaml similarity index 100% rename from examples/ros/smb_estimator_graph/config/smb_specific/smb_graph_params.yaml rename to ros/examples/smb_estimator_graph/config/smb_specific/smb_graph_params.yaml diff --git a/examples/ros/smb_estimator_graph/include/smb_estimator_graph/SmbEstimator.h b/ros/examples/smb_estimator_graph/include/smb_estimator_graph/SmbEstimator.h similarity index 100% rename from examples/ros/smb_estimator_graph/include/smb_estimator_graph/SmbEstimator.h rename to ros/examples/smb_estimator_graph/include/smb_estimator_graph/SmbEstimator.h diff --git a/examples/ros/smb_estimator_graph/include/smb_estimator_graph/SmbStaticTransforms.h b/ros/examples/smb_estimator_graph/include/smb_estimator_graph/SmbStaticTransforms.h similarity index 100% rename from examples/ros/smb_estimator_graph/include/smb_estimator_graph/SmbStaticTransforms.h rename to ros/examples/smb_estimator_graph/include/smb_estimator_graph/SmbStaticTransforms.h diff --git a/examples/ros/smb_estimator_graph/include/smb_estimator_graph/constants.h b/ros/examples/smb_estimator_graph/include/smb_estimator_graph/constants.h similarity index 100% rename from examples/ros/smb_estimator_graph/include/smb_estimator_graph/constants.h rename to ros/examples/smb_estimator_graph/include/smb_estimator_graph/constants.h diff --git a/examples/ros/smb_estimator_graph/launch/smb_estimator_graph.launch b/ros/examples/smb_estimator_graph/launch/smb_estimator_graph.launch similarity index 100% rename from examples/ros/smb_estimator_graph/launch/smb_estimator_graph.launch rename to ros/examples/smb_estimator_graph/launch/smb_estimator_graph.launch diff --git a/examples/ros/smb_estimator_graph/launch/smb_estimator_graph_replay.launch b/ros/examples/smb_estimator_graph/launch/smb_estimator_graph_replay.launch similarity index 100% rename from examples/ros/smb_estimator_graph/launch/smb_estimator_graph_replay.launch rename to ros/examples/smb_estimator_graph/launch/smb_estimator_graph_replay.launch diff --git a/examples/ros/smb_estimator_graph/package.xml b/ros/examples/smb_estimator_graph/package.xml similarity index 100% rename from examples/ros/smb_estimator_graph/package.xml rename to ros/examples/smb_estimator_graph/package.xml diff --git a/examples/ros/smb_estimator_graph/rviz/lidar_estimation.rviz b/ros/examples/smb_estimator_graph/rviz/lidar_estimation.rviz similarity index 100% rename from examples/ros/smb_estimator_graph/rviz/lidar_estimation.rviz rename to ros/examples/smb_estimator_graph/rviz/lidar_estimation.rviz diff --git a/examples/ros/smb_estimator_graph/scripts/remove_map_and_odom_from_bag.py b/ros/examples/smb_estimator_graph/scripts/remove_map_and_odom_from_bag.py similarity index 100% rename from examples/ros/smb_estimator_graph/scripts/remove_map_and_odom_from_bag.py rename to ros/examples/smb_estimator_graph/scripts/remove_map_and_odom_from_bag.py diff --git a/examples/ros/smb_estimator_graph/src/lib/SmbEstimator.cpp b/ros/examples/smb_estimator_graph/src/lib/SmbEstimator.cpp similarity index 100% rename from examples/ros/smb_estimator_graph/src/lib/SmbEstimator.cpp rename to ros/examples/smb_estimator_graph/src/lib/SmbEstimator.cpp diff --git a/examples/ros/smb_estimator_graph/src/lib/SmbStaticTransforms.cpp b/ros/examples/smb_estimator_graph/src/lib/SmbStaticTransforms.cpp similarity index 100% rename from examples/ros/smb_estimator_graph/src/lib/SmbStaticTransforms.cpp rename to ros/examples/smb_estimator_graph/src/lib/SmbStaticTransforms.cpp diff --git a/examples/ros/smb_estimator_graph/src/lib/readParams.cpp b/ros/examples/smb_estimator_graph/src/lib/readParams.cpp similarity index 100% rename from examples/ros/smb_estimator_graph/src/lib/readParams.cpp rename to ros/examples/smb_estimator_graph/src/lib/readParams.cpp diff --git a/examples/ros/smb_estimator_graph/src/smb_estimator_node.cpp b/ros/examples/smb_estimator_graph/src/smb_estimator_node.cpp similarity index 100% rename from examples/ros/smb_estimator_graph/src/smb_estimator_node.cpp rename to ros/examples/smb_estimator_graph/src/smb_estimator_node.cpp diff --git a/ros2/examples/README.md b/ros2/examples/README.md new file mode 100644 index 00000000..f3e9f667 --- /dev/null +++ b/ros2/examples/README.md @@ -0,0 +1 @@ +# ROS2 Examples \ No newline at end of file From 7962bb8a1f547d6f393b1b35d809ee39659992b8 Mon Sep 17 00:00:00 2001 From: Julian Date: Tue, 27 May 2025 10:41:06 +0200 Subject: [PATCH 31/60] fix two more examples, now full graph_msf_ros_examples meta package is building --- ros/examples/atn_position3_fuser/CMakeLists.txt | 2 +- ros/examples/atn_position3_fuser/package.xml | 4 ++-- ros/examples/excavator_holistic_graph/CMakeLists.txt | 2 +- ros/examples/excavator_holistic_graph/package.xml | 4 ++-- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ros/examples/atn_position3_fuser/CMakeLists.txt b/ros/examples/atn_position3_fuser/CMakeLists.txt index 778baed5..2f00b3a7 100644 --- a/ros/examples/atn_position3_fuser/CMakeLists.txt +++ b/ros/examples/atn_position3_fuser/CMakeLists.txt @@ -27,7 +27,7 @@ endif () # Catkin dependencies ------------------------------------------------------------------------------------------------- set(CATKIN_PACKAGE_DEPENDENCIES - graph_msf + graph_msf_catkin graph_msf_ros geometry_msgs nav_msgs diff --git a/ros/examples/atn_position3_fuser/package.xml b/ros/examples/atn_position3_fuser/package.xml index 538de3a2..f28407b6 100644 --- a/ros/examples/atn_position3_fuser/package.xml +++ b/ros/examples/atn_position3_fuser/package.xml @@ -12,7 +12,7 @@ Julian Nubert catkin - graph_msf + graph_msf_catkin graph_msf_ros geometry_msgs nav_msgs @@ -23,7 +23,7 @@ std_srvs message_filters - graph_msf + graph_msf_catkin graph_msf_ros geometry_msgs nav_msgs diff --git a/ros/examples/excavator_holistic_graph/CMakeLists.txt b/ros/examples/excavator_holistic_graph/CMakeLists.txt index a2f18474..910cf5b4 100644 --- a/ros/examples/excavator_holistic_graph/CMakeLists.txt +++ b/ros/examples/excavator_holistic_graph/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.16) project(excavator_holistic_graph) set(CATKIN_PACKAGE_DEPENDENCIES - graph_msf + graph_msf_catkin graph_msf_ros geometry_msgs nav_msgs diff --git a/ros/examples/excavator_holistic_graph/package.xml b/ros/examples/excavator_holistic_graph/package.xml index b9dd1c9a..b94526d3 100644 --- a/ros/examples/excavator_holistic_graph/package.xml +++ b/ros/examples/excavator_holistic_graph/package.xml @@ -12,7 +12,7 @@ Julian Nubert catkin - graph_msf + graph_msf_catkin graph_msf_ros geometry_msgs nav_msgs @@ -23,7 +23,7 @@ std_srvs message_filters - graph_msf + graph_msf_catkin graph_msf_ros geometry_msgs nav_msgs From ff6cd78fa85a66898a024130251836cba2074ad6 Mon Sep 17 00:00:00 2001 From: Julian Date: Tue, 27 May 2025 10:57:55 +0200 Subject: [PATCH 32/60] make sure the gtsam libaries can be found --- docker/submodules/gtsam.sh | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/docker/submodules/gtsam.sh b/docker/submodules/gtsam.sh index 3c7fc92e..0746a875 100644 --- a/docker/submodules/gtsam.sh +++ b/docker/submodules/gtsam.sh @@ -1,5 +1,9 @@ #!/bin/bash +# Exit if a single command fails +set -e + +# Install GTSAM mkdir -p /software \ && cd /software \ && git clone https://github.com/borglab/gtsam.git \ @@ -14,4 +18,7 @@ mkdir -p /software \ -DGTSAM_USE_QUATERNIONS=ON \ -DGTSAM_USE_SYSTEM_EIGEN=ON \ /software/gtsam \ - && make install -j$(nproc) \ No newline at end of file + && make install -j$(nproc) + + # Update library cache + ldconfig \ No newline at end of file From 442c3f923c69a29770333010ae98bc484b83f972 Mon Sep 17 00:00:00 2001 From: Julian Date: Tue, 27 May 2025 11:02:55 +0200 Subject: [PATCH 33/60] update to holistic_fusion --- docker/ros1_noetic/entrypoint.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/ros1_noetic/entrypoint.sh b/docker/ros1_noetic/entrypoint.sh index 82c14dbe..47945995 100644 --- a/docker/ros1_noetic/entrypoint.sh +++ b/docker/ros1_noetic/entrypoint.sh @@ -12,7 +12,7 @@ # Authors: Vassilios Tsounis, tsounisv@ethz.ch # Julian Nubert, nubertj@ethz.ch #============================================================================= -figlet m545 +figlet holistic_fusion #== # Log into the container as the host user #== From 2d0b24f83c5b2dcb40ff087eab8fee8b3ea7f946 Mon Sep 17 00:00:00 2001 From: Julian Date: Tue, 27 May 2025 22:20:45 +0200 Subject: [PATCH 34/60] building with colcon without any ament specific package in graph_msf using modern cmake --- graph_msf/CMakeLists.txt | 78 +++++++++++-------- graph_msf/graph_msfConfig.cmake.in | 9 +++ ros/graph_msf_catkin/package.xml | 5 ++ .../smb_estimator_graph_ros2/CMakeLists.txt | 2 +- ros2/graph_msf_ros2/CMakeLists.txt | 14 +++- 5 files changed, 74 insertions(+), 34 deletions(-) create mode 100644 graph_msf/graph_msfConfig.cmake.in diff --git a/graph_msf/CMakeLists.txt b/graph_msf/CMakeLists.txt index f9d9d647..21041d4c 100644 --- a/graph_msf/CMakeLists.txt +++ b/graph_msf/CMakeLists.txt @@ -1,25 +1,11 @@ cmake_minimum_required(VERSION 3.16) project(graph_msf) -message("Building graph_msf library -----------------------------") +message("Building ${PROJECT_NAME} library -----------------------------") ## Compile as C++17, supported in ROS Noetic and newer add_compile_options(-std=c++17) -# If build variables not set, set them to default values -if (NOT DEFINED CMAKE_BUILD_LIBDIR) - set(CMAKE_BUILD_LIBDIR ${CMAKE_BINARY_DIR}/lib) -endif () -if (NOT DEFINED CMAKE_BUILD_BINDIR) - set(CMAKE_BUILD_BINDIR ${CMAKE_BINARY_DIR}/bin) -endif () -if (NOT DEFINED CMAKE_BUILD_INCLUDE_DIR) - set(CMAKE_BUILD_INCLUDE_DIR ${CMAKE_BINARY_DIR}/include) -endif () -message("CMAKE_BUILD_LIBDIR: ${CMAKE_BUILD_LIBDIR}") -message("CMAKE_BUILD_BINDIR: ${CMAKE_BUILD_BINDIR}") -message("CMAKE_BUILD_INCLUDE_DIR: ${CMAKE_BUILD_INCLUDE_DIR}") - # Find dependencies ---------------------------------------------------------------------------------------------------- find_package(Eigen3 REQUIRED) find_package(ament_cmake REQUIRED) @@ -85,6 +71,13 @@ target_link_libraries(${PROJECT_NAME} metis-gtsam ) +# Include directories for the library +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ +) + # Add clang tooling find_package(cmake_clang_tools QUIET) if (cmake_clang_tools_FOUND AND NOT DEFINED NO_CLANG_TOOLING) @@ -96,30 +89,51 @@ if (cmake_clang_tools_FOUND AND NOT DEFINED NO_CLANG_TOOLING) ) endif() +############# +## Install ## +############# + # Export the include directories and linked libraries -install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME}-targets # Export the target for downstream usage - ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}Targets # Export the target for downstream usage + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# Install the include directory +install(DIRECTORY include/ + DESTINATION include ) # Install the export file for downstream projects -install(EXPORT ${PROJECT_NAME}-targets - FILE ${PROJECT_NAME}Config.cmake - DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME} +install(EXPORT ${PROJECT_NAME}Targets + FILE ${PROJECT_NAME}Targets.cmake + NAMESPACE ${PROJECT_NAME}:: + DESTINATION share/${PROJECT_NAME}/cmake ) -# Install the include directory -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/${PROJECT_NAME} +# Version +include(CMakePackageConfigHelpers) +write_basic_package_version_file( + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" + VERSION 1.0.0 + COMPATIBILITY SameMajorVersion +) + +# Configure +configure_package_config_file( + "${PROJECT_NAME}Config.cmake.in" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + INSTALL_DESTINATION share/${PROJECT_NAME}/cmake ) -# Add GTSAM include directories to the exported interface -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ - ${EIGEN3_INCLUDE_DIR} +# Files +install(FILES + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" + DESTINATION share/${PROJECT_NAME}/cmake ) -message("Finished building graph_msf library -----------------------------") +message("Finished building ${PROJECT_NAME} library -----------------------------") diff --git a/graph_msf/graph_msfConfig.cmake.in b/graph_msf/graph_msfConfig.cmake.in new file mode 100644 index 00000000..3a456536 --- /dev/null +++ b/graph_msf/graph_msfConfig.cmake.in @@ -0,0 +1,9 @@ +@PACKAGE_INIT@ + +include("${CMAKE_CURRENT_LIST_DIR}/graph_msfTargets.cmake") + +# Optionally set legacy-style variables (only if needed) +# set(graph_msf_INCLUDE_DIRS "@CMAKE_INSTALL_INCLUDEDIR@") + +# Set up version compatibility (optional if you use a version file) +# include("${CMAKE_CURRENT_LIST_DIR}/graph_msfConfigVersion.cmake") \ No newline at end of file diff --git a/ros/graph_msf_catkin/package.xml b/ros/graph_msf_catkin/package.xml index f7455d0e..9e143e2d 100644 --- a/ros/graph_msf_catkin/package.xml +++ b/ros/graph_msf_catkin/package.xml @@ -8,5 +8,10 @@ BSD Julian Nubert + catkin + + + gtsam + eigen diff --git a/ros2/examples/smb_estimator_graph_ros2/CMakeLists.txt b/ros2/examples/smb_estimator_graph_ros2/CMakeLists.txt index 1361a335..1435f97a 100644 --- a/ros2/examples/smb_estimator_graph_ros2/CMakeLists.txt +++ b/ros2/examples/smb_estimator_graph_ros2/CMakeLists.txt @@ -55,7 +55,7 @@ add_library(${PROJECT_NAME} target_link_libraries(${PROJECT_NAME} gtsam gtsam_unstable - ${graph_msf_LIBRARIES} + graph_msf::graph_msf ${graph_msf_ros2_LIBRARIES} ${GLOG_LIBRARIES} ${gflags_LIBRARIES} diff --git a/ros2/graph_msf_ros2/CMakeLists.txt b/ros2/graph_msf_ros2/CMakeLists.txt index ed46849d..3bc8a1c7 100644 --- a/ros2/graph_msf_ros2/CMakeLists.txt +++ b/ros2/graph_msf_ros2/CMakeLists.txt @@ -21,6 +21,13 @@ find_package(std_srvs REQUIRED) find_package(visualization_msgs REQUIRED) find_package(graph_msf REQUIRED) +# Show path of graph_msf +if (graph_msf_FOUND) + message(STATUS "graph_msf was found!") +else() + message(FATAL_ERROR "graph_msf NOT found!") +endif() + # Color (Optional) if (NOT WIN32) string(ASCII 27 Esc) @@ -33,7 +40,6 @@ endif () include_directories( include ${EIGEN3_INCLUDE_DIR} - ${graph_msf_INCLUDE_DIRS} ) # Library @@ -62,6 +68,12 @@ ament_target_dependencies(${PROJECT_NAME} graph_msf rosidl_default_runtime ) +target_link_libraries(${PROJECT_NAME} + graph_msf::graph_msf +) +# Print the include directories for debugging +get_target_property(INC_DIR graph_msf::graph_msf INTERFACE_INCLUDE_DIRECTORIES) +message("Include dir for graph_msf::graph_msf: ${INC_DIR}") # Python Scripts ---------------------------------------------------------------- install(PROGRAMS From d96dd77eff0fc2d5d8bd068ecaf3f2131fc3286d Mon Sep 17 00:00:00 2001 From: Julian Date: Tue, 27 May 2025 22:34:59 +0200 Subject: [PATCH 35/60] remove unnecessary ament dependency messing up catkin build --- graph_msf/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/graph_msf/CMakeLists.txt b/graph_msf/CMakeLists.txt index 21041d4c..c26e864e 100644 --- a/graph_msf/CMakeLists.txt +++ b/graph_msf/CMakeLists.txt @@ -8,7 +8,6 @@ add_compile_options(-std=c++17) # Find dependencies ---------------------------------------------------------------------------------------------------- find_package(Eigen3 REQUIRED) -find_package(ament_cmake REQUIRED) find_package(GTSAM REQUIRED) find_package(GTSAM_UNSTABLE REQUIRED) find_package(Python3 REQUIRED COMPONENTS Development) From 105bfd3ec48de8d3f8d2563c3f790935b57ce8b7 Mon Sep 17 00:00:00 2001 From: Julian Nubert Date: Wed, 11 Jun 2025 12:20:05 +0200 Subject: [PATCH 36/60] Julian/feature/ros2 fixes (#12) * reduce ament further * Update IMU subscription topic in GraphMsfRos2 to '/imu_topic' * Update lioOdometryRate in smb_graph_params.yaml from 10 to 100 * fix sim time arg * Remove obsolete YAML configuration files for compslam and dumped parameters in smb_estimator_graph_ros2. * Update realTimeSmootherLag parameter in core_graph_params.yaml from 2.0 to 10.0 * Remove obsolete YAML configuration files for compslam and dumped parameters in smb_estimator_graph_ros2. * Add odometry flags and update SmbStaticTransforms logic - Introduced flags for LIO, VIO, and wheel odometry in SmbStaticTransforms. - Updated SmbEstimator to set these flags during initialization. - Modified findTransformations method to conditionally perform transformations based on the new flags. * remove python dependency, as it creates issues when reinitialziing workspace --------- Co-authored-by: dishtaweera --- graph_msf/CMakeLists.txt | 4 +- .../compslam/compslam_lio_RS16_params.yaml | 109 ---- .../dumped_params/2023-06-13-13-15-57.yaml | 523 ------------------ .../smb_estimator_graph_ros2/CMakeLists.txt | 42 +- .../compslam/compslam_lio_RS16_params.yaml | 109 ---- .../config/core/core_graph_params.yaml | 2 +- .../dumped_params/2023-06-13-13-15-57.yaml | 523 ------------------ .../config/smb_specific/smb_graph_params.yaml | 2 +- .../SmbStaticTransforms.h | 12 + .../launch/smb_estimator_graph_sim.launch.py | 2 +- .../src/lib/SmbEstimator.cpp | 10 + .../src/lib/SmbStaticTransforms.cpp | 89 +-- ros2/graph_msf_ros2/CMakeLists.txt | 121 ++-- .../graph_msf_ros2Config.cmake.in | 9 + ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp | 2 +- 15 files changed, 182 insertions(+), 1377 deletions(-) delete mode 100644 ros/examples/smb_estimator_graph/config/compslam/compslam_lio_RS16_params.yaml delete mode 100644 ros/examples/smb_estimator_graph/config/dumped_params/2023-06-13-13-15-57.yaml delete mode 100644 ros2/examples/smb_estimator_graph_ros2/config/compslam/compslam_lio_RS16_params.yaml delete mode 100644 ros2/examples/smb_estimator_graph_ros2/config/dumped_params/2023-06-13-13-15-57.yaml create mode 100644 ros2/graph_msf_ros2/graph_msf_ros2Config.cmake.in diff --git a/graph_msf/CMakeLists.txt b/graph_msf/CMakeLists.txt index 59e8dcc0..384d6d67 100644 --- a/graph_msf/CMakeLists.txt +++ b/graph_msf/CMakeLists.txt @@ -24,7 +24,6 @@ message("CMAKE_BUILD_INCLUDE_DIR: ${CMAKE_BUILD_INCLUDE_DIR}") find_package(Eigen3 REQUIRED) find_package(GTSAM REQUIRED) find_package(GTSAM_UNSTABLE REQUIRED) -find_package(Python3 REQUIRED COMPONENTS Development) find_package(tf2_eigen REQUIRED) message("Eigen Version:: ${EIGEN3_VERSION_STRING}") @@ -52,6 +51,7 @@ message("GTSAM Path:" ${GTSAM_DIR}) ########### ## Build ## ########### + set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) @@ -76,6 +76,8 @@ add_library(${PROJECT_NAME} SHARED src/lib/TrajectoryAlignmentHandler.cpp src/lib/NavState.cpp ) +# Exported alias target +add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME}) # Link GTSAM and other dependencies to the graph_msf library target_link_libraries(${PROJECT_NAME} diff --git a/ros/examples/smb_estimator_graph/config/compslam/compslam_lio_RS16_params.yaml b/ros/examples/smb_estimator_graph/config/compslam/compslam_lio_RS16_params.yaml deleted file mode 100644 index 3bfdebc8..00000000 --- a/ros/examples/smb_estimator_graph/config/compslam/compslam_lio_RS16_params.yaml +++ /dev/null @@ -1,109 +0,0 @@ -#Common -loamVerbosity: 0 #Debug Output, 0-disable, higher for more verbose output -scanPeriod: 0.1 #Expected scan period for input pointcloud in seconds. Used of Distortion correction -ioRatio: 2 #Ratio of publish rate of LaserOdometry w.r.t input PCL rate. LO is calculated for each PCL but published at slower rate to Mapping, default 2 -lidarFrame: rslidar #LiDAR frame name - used for LiDAR-to-Sensor transform lookup -rotateInputCloud: false #Flag to rotate input cloud before estimating odometry/map so produced resuts are ROS frame aligned irrespective of LiDAR mounting orientation -inputCloudRotation: [0.0, 0.0, 0.0] #Rotation applied to input cloud - ORDER YPR(radians) #90 deg = 1.5707963268 rad - -#External Prior/Transform Input -forceExtPriorUseMapping : false #Flag to force use of FULL external prior instead of LaserOdometry TRANSLATION Only Estimate -extPriorAvailable: true #Flag to check if 'Primary' external prior is available -extOdomFrame: imu #External Prior odometry frame name -extFixedFrame: odom #External Prior fixed frame name -extSensorFrame: imu_link #External Prior sensor frame name -extOdomTimeOffset : 0.0 #Timeoffset (seconds) between LiDAR pointcloud and external source -fallbackExtPriorAvailable: false #Flag to check if 'Fallback ' external prior is available -fallbackExtOdomFrame: vio_imu #Fallback External Prior odometry frame name -fallbackExtFixedFrame: vio_imu_init #Fallback External Prior fixed frame name -fallbackExtSensorFrame: imu_sensor_frame #Fallback External Prior sensor frame name -fallbackExtOdomTimeOffset: 0.00 #Timeoffset (seconds) between LiDAR pointcloud and Fallbackexternal source - -#MultiScanRegistration -lidar: VLP-16 #Choose LiDAR type - options: VLP-16 HDL-32 HDL-64E O1-16 O1-64 Bperl-32 -useCloudIntensityandRingFields : false #Flag to use input pointcloud intensity or ring fields. Converts to custom PointXYZIR instead of PointXYZ. True for Bpearl -uniformLidarRingDistance: true # Whether the LiDAR rings are uniformly distributed in elevation, does not hold for e.g. Helios -ringAngleTolerance: 0.5 # [deg], Tolerance up to which angle rays are considered to belong to same ring, only impact if argument before is false -imuHistorySize: 800 #IMU Message Buffer Size , default: 200 -minRange: 0.1 #Minimum Range of useful points, default: 0.1 -maxRange: 80.0 #Maximum Range of useful points, default: 80 -featureRegions: 6 #Number of Azimuth Regions Pointcloud is divided, default:6 -curvatureRegion: 5 #Number of neigboring points on a scan line on each side of a point used for calculating curvature of that point, default 5 -maxCornerSharp: 2 #Number of Sharp Features per scan line in each curvatureRegion, default:2 -maxCornerLessSharp: 20 #Number of Less Sharp Features per scan line in each curvatureRegion, default:10*maxCornerLessSharp -maxSurfaceFlat: 4 #Number of FlatFeatures per scan line in each curvatureRegion, default:4 -surfaceCurvatureThreshold: 0.1 #Threshold above which feature is categorized Sharp, default 0.1 -lessFlatFilterSize: 0.2 #Leaf Size for downsampling remaing pointcloud after feature selection, default 0.2 -checkInvalidFeatureRegions: false -publishDebugArrowsToRemovedFeatures: false -azimuthBoundaries: [1.1,1.5,1.7,2.2] #ANYmal - -#LaserOdometry -undistortInputCloud: true #If true External Prior or Motion Model will be used for LiDAR Ego Motion Compensation of input cloud -odomMaxIterations: 25 #Maximum Number of LO optimization iterations, default 25 -odomMinIterations: 1 #Minimum Number of LO optimization iterations, default 4, (set 0 to perform only first iteration at minimum) -odomDeltaTAbort: 0.05 #Translation threshold for optimization convergence, deafult 0.1 (m) -odomDeltaRAbort: 0.05 #Rotation threshold for optimization convergence, deafult 0.1 (deg) -odomDegenEigVal: 30 #Minimum eignevalue threshold for determining degeneracy of LO optimization, default 30 -odomRelativeTranslationMax: 0.8 #Max translation threshold between two pointclouds for external odometry to be considered valid input. Determined w.r.t set max robot movement speed -odomRelativeRotationMax: 0.1 #Max rotation threshold between two pointclouds from external odometry to be considered valid input. Determined w.r.t set max robot movement speed - -#LaserMapping -mapMaxIterations: 10 #Maximum Number of LM optimization iterations, default 10 -mapMinIterations: 1 #Minimum Number of LM optimization iterations, default 1, (set 0 to perform only first iteration at minimum) -mapDeltaTAbort: 0.05 #Translation threshold for optimization convergence, deafult 0.05 (m) -mapDeltaRAbort: 0.05 #Rotation threshold for optimization convergence, deafult 0.05 (deg) -cornerFilterSize: 0.2 #Leaf Size for downsampling current CORNER pointcloud before merging in map, default 0.2 -surfaceFilterSize: 0.2 #Leaf Size for downsampling current FLAT/SURFACE pointcloud before merging in map, default 0.2 -mapVisFilterSize: 0.4 #Leaf Size for downsampling current visualization Map pointcloud, default 0.5 -rndSampleMapVisNoOfSamples: 100000 #Number of Samples for random sampling of the visualized map cloud, default 100000 -mapCubeSize: 10.0 #Size of Cube/Voxel used for saving internal map (meters), default 50 -mapDimensionsInCubes: [101,21,101] #WxHxD of internal map in CUBE UNITS, default:[21,11,21] #Width,Height,Depth -mapStartLocationInCubes: [50,10,50] #Robot start position in internal map in CUBE UNITS, default:[10,5,10] #Width,Height,Depth -numNeighborSubmapCubes: 4 #Number of Neigboring cubes in +/- direction along each axis to build submap for matching -mapDegenEigVal: 10 #Minimum eignevalue threshold for determining degeneracy of LM optimization, default 30 -mapPriorNormThresh: 0.5 #Max translation threshold between two pointclouds from external odometry to be considered valid input. Determined w.r.t set max robot movement speed -useSavedSubmapsForInitialization: false #Use saved submap on disc for initialization -submapLocalizationInitGuess: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #Initialization guess of robot location in submap - ORDER: XYZ(meters) - YPR(radians) -waitAndUseExternalSubmapsForInit: false #Wait for external submaps before initlization. Trigger external submaps through service call -submapsRequestServiceCall: "" #Name of service call to request submaps from other node (xyz/share_submaps) -useExternalSubmapOriginForInit: false #Use origin of external submaps as initlization guess for co-localization instead of current external pose (default: false) -forcePathPublishing: true #Force ROS path message publishing for mapping odometry -mapPublishRateDivider: 20 #Publishes full map at 1.0/(scanPeriod * ioRatio * mapPublishRateDivider), default: 20(0.25 Hz) -useExtRotationAlignment: true #Use external fixed & map frame to detemine gravity misalignment, default: true -submapVisCropRange: 0 #Range of cropping for submap visualization (set 0 to disable) - -#Factor Graph -imuFrame: imu_link #Frame of IMU used in integrator - Used for lookup transform with lidarFrame -imuRate: 400 #Rate of IMU input (Hz) used for initialization of gravity, initial Roll/Pitch and gyro bias -imuTimeForInit: 0.5 #Time interval of IMU measurements used for initialization -imuTimeOffset: 0.0 #seconds #Offset between IMU and LiDAR Measurements - Depending on LiDAR timestamp first(+0.05) or last(-0.05) -initGraphRollPitchFromIMU: true #Initialize graph roll/pitch from IMU -globalZaxisPointsUp: true #Z-axis of global coordinate system pointing up -accNoiseDensity: 7.84e-04 #Continuous-time "Covariance" of accelerometer -accBiasRandomWalk: 1.0e-04 #Continuous-time "Covariance" describing accelerometer bias random walk (biasAccCovariance) -gyrNoiseDensity: 3.4906585e-5 #Continuous-time "Covariance" of gyroscope measurements -gyrBiasRandomWalk: 1.0e-04 #Continuous-time "Covariance" describing gyroscope bias random walk (biasOmegaCovariance) -imuIntegrationCovariance: 1.0e-08 #Continuous-time "Covariance" describing integration uncertainty -imuBiasAccOmegaInt: 1.0 #Covariance of bias used for pre-integration -accBiasPrior: [0.0, 0.0, 0.0] #Prior/starting value of accelerometer bias -gyrBiasPrior: [0.0, 0.0, 0.0] #Prior/starting value of gyroscope bias -smootherLag: 3.0 #Lag of fixed lag smoother[seconds] -additonalIterations: 3 #Additional iterations of graph optimizer after update with new factors -positionReLinTh: 0.05 #Position linearization threshold -rotationReLinTh: 0.05 #Rotation linearization threshold -velocityReLinTh: 0.1 #Linear Velocity linearization threshold -accBiasReLinTh: 0.1 #Accelerometer bias linearization threshold -gyrBiasReLinTh: 0.1 #Gyroscope bias linearization threshold -relinearizeSkip: 1 -enableRelinearization: true -evaluateNonlinearError: false -cacheLinearizedFactors: true -findUnusedFactorSlots: false -enablePartialRelinearizationCheck: true -enableDetailedResults: false -poseBetweenNoise: [0.5, 0.5, 0.05, 0.1, 0.1, 0.1] #Noise of add between factor -ORDER RPY(rad) - XYZ(meters) -zeroMotionDetection: false #Detect and Add Zero Motion Factors(Zero delta Pose and Velocity) -zeroMotionThreshold: 0.01 #Zero motion threshold in meters (Currently only motion detection is translation only) -minZeroMoitionDetections: 10 #Minimum number of consective zero motions detected before factors are added -gravityRollPitchFactors: false #Add Gravity-aligned Roll-Pitch from IMU when in Zero motion (only works if Zero-Motion Factors are added) diff --git a/ros/examples/smb_estimator_graph/config/dumped_params/2023-06-13-13-15-57.yaml b/ros/examples/smb_estimator_graph/config/dumped_params/2023-06-13-13-15-57.yaml deleted file mode 100644 index 403a3779..00000000 --- a/ros/examples/smb_estimator_graph/config/dumped_params/2023-06-13-13-15-57.yaml +++ /dev/null @@ -1,523 +0,0 @@ -control: - joint_state_controller: - publish_rate: 50 - type: joint_state_controller/JointStateController - joy_teleop: - joy_node: - autorepeat_rate: 20 - deadzone: 0.1 - dev: /dev/input/js0 - teleop_twist_joy: - axis_angular: 0 - axis_linear: 1 - enable_button: 4 - enable_turbo_button: 5 - scale_angular: 1.2 - scale_angular_turbo: 1.2 - scale_linear: 1.6 - scale_linear_turbo: 1.0 - lowlevel_controller: - LF_WHEEL_JOINT: - has_acceleration_limits: true - has_effort_limits: true - has_jerk_limits: true - has_position_limits: false - has_velocity_limits: true - max_acceleration: 5.0 - max_effort: 1.0 - max_jerk: 100.0 - max_velocity: 2.0 - LF_WHEEL_JOINT_dc_controller: - antiwindup: true - d: 0.0 - i: 150.0 - i_clamp: 300 - i_clamp_max: 300.0 - i_clamp_min: -300.0 - p: 50.0 - publish_state: true - LF_WHEEL_velocity_controller: - joint: LF_WHEEL_JOINT - pid: - d: 10.0 - i: 0.01 - p: 100.0 - type: velocity_controllers/JointVelocityController - RF_WHEEL_JOINT: - has_acceleration_limits: true - has_effort_limits: true - has_jerk_limits: true - has_position_limits: false - has_velocity_limits: true - max_acceleration: 5.0 - max_effort: 1.0 - max_jerk: 100.0 - max_velocity: 2.0 - RF_WHEEL_JOINT_dc_controller: - antiwindup: true - d: 0.0 - i: 150.0 - i_clamp: 300 - i_clamp_max: 300.0 - i_clamp_min: -300.0 - p: 50.0 - publish_state: true - RF_WHEEL_velocity_controller: - joint: RF_WHEEL_JOINT - pid: - d: 10.0 - i: 0.01 - p: 100.0 - type: velocity_controllers/JointVelocityController - WHEEL_JOINT_ff_param: - ff_general: 25 - ff_pure_rotation: 100 - robot_joint_publisher: - publish_rate: 50 - type: joint_state_controller/JointStateController - smb: - ang_vel_scale: 1.8 - hardware_interface: - joints: - - LF_WHEEL_JOINT - - RF_WHEEL_JOINT - loop_hz: 400 - lin_vel_scale: 1.8 - smb_diff_drive: - angular: - z: - has_acceleration_limits: true - has_velocity_limits: true - max_acceleration: 5.0 - max_velocity: 2.0 - base_frame_id: base_link - cmd_vel_timeout: 0.25 - enable_odom_tf: false - estimate_velocity_from_position: false - left_wheel: - - LF_WHEEL_JOINT - left_wheel_radius_multiplier: 1.0 - linear: - x: - has_acceleration_limits: true - has_velocity_limits: true - max_acceleration: 5.0 - max_velocity: 2.0 - pose_covariance_diagonal: - - 0.001 - - 0.001 - - 0.001 - - 0.001 - - 0.001 - - 0.03 - publish_rate: 50.0 - right_wheel: - - RF_WHEEL_JOINT - right_wheel_radius_multiplier: 1.0 - twist_covariance_diagonal: - - 0.001 - - 0.001 - - 0.001 - - 0.001 - - 0.001 - - 0.03 - type: diff_drive_controller/DiffDriveController - velocity_rolling_window_size: 2 - wheel_radius_multiplier: 1.0 - wheel_separation_multiplier: 1.0 - smb_lowlevel_controller: - command_smb: true - control_namespace: /control - controller_namespace: lowlevel_controller - port: /dev/ttyUSB0 - smb: - ang_vel_scale: 1.8 - hardware_interface: - joints: - - LF_WHEEL_JOINT - - RF_WHEEL_JOINT - loop_hz: 400 - lin_vel_scale: 1.8 - smb_robot_state_publisher: - publish_frequency: 50 - use_tf_static: true - twist_mux: - locks: - - name: e_stop - priority: 255 - timeout: 0.0 - topic: joy_teleop/e_stop - - name: e_stop_opc - priority: 200 - timeout: 0.0 - topic: opc/e_stop - topics: - - name: RC - priority: 15 - timeout: 0.5 - topic: smb_lowlevel_controller/rc_twist - - name: joy - priority: 10 - timeout: 0.5 - topic: joy_teleop/cmd_vel - - name: keyboard - priority: 9 - timeout: 0.5 - topic: keyboard_teleop/cmd_vel - - name: joy_opc - priority: 8 - timeout: 0.5 - topic: opc/cmd_vel - - name: mpc_command_twist - priority: 5 - timeout: 0.2 - topic: smb_mpc/command_twist - - name: teb_planner_twist - priority: 3 - timeout: 1.0 - topic: teb_planner_twist -odometry: - tracking_camera_odometry_conversion: - in_odom_frame: tracking_camera_pose_frame - in_sensor_frame: tracking_camera_pose_frame - out_odom_frame: tracking_camera_odom - out_sensor_frame: base_link -rosdistro: 'noetic - - ' -roslaunch: - uris: - host_smb_263_nuc__35755: http://smb-263-nuc:35755/ -rosserial_python: - baud: 250000 - port: /dev/versavis -rosversion: '1.16.0 - - ' -run_id: d353b820-09d9-11ee-9f1a-13a7083ee06f -serial_node: - baud: 115200 - port: /dev/smb-power -simulation: false -smb_description: "\n\n\n\n\n\n \n \n\ - \ \n \n \n \n \n \n \n \n\ - \ \n \n\ - \ \n \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \ - \ \n \n \n \n \n \n \n \n \n \n \n \n \n \n\ - \ Gazebo/Grey\n \n \n Gazebo/Red\n \n \n \n \n\ - \ \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \ - \ \n \n \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n 10000000.0\n\ - \ 1.0\n 0.005\n 100.0\n\ - \ Gazebo/DarkGrey\n \n \n transmission_interface/SimpleTransmission\n\ - \ \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ \n \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ 1\n \n \n\ - \ \n \n \n \n \n\ - \ \n \n \n \n \ - \ \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n 10000000.0\n\ - \ 1.0\n 0.005\n 100.0\n\ - \ Gazebo/DarkGrey\n \n \n transmission_interface/SimpleTransmission\n\ - \ \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ \n \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ 1\n \n \n\ - \ \n \n \n \n \n\ - \ \n \n \n \n \ - \ \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n 10000000.0\n\ - \ 1.0\n 0.005\n 100.0\n\ - \ Gazebo/DarkGrey\n \n \n transmission_interface/SimpleTransmission\n\ - \ \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ \n \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ 1\n \n \n\ - \ \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n 10000000.0\n\ - \ 1.0\n 0.005\n 100.0\n\ - \ Gazebo/DarkGrey\n \n \n transmission_interface/SimpleTransmission\n\ - \ \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ \n \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ 1\n \n \n\ - \ \n \n \n \n \n \n \n\ - \ \n \n \n \ - \ \n \n \n \ - \ \n \n \n \ - \ \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n \n \n Gazebo/Orange\n \n \n true\n \n\ - \ \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \ - \ \n \n \n \n \n \n \n \n Gazebo/Red\n \n \n \n \n \n \n \n \n true\n\ - \ \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n Gazebo/Grey\n\ - \ \n \n \n \n \n \n \n \n\ - \ \n \n \n \n\ - \ \n \n \n \ - \ \n \n \n \n \n \ - \ \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n \ - \ \n \n \n \n \n \ - \ \n \n \n Gazebo/Black\n\ - \ \n \n \n \n \n \n \n \n true\n\ - \ \n \n \n \n \n \n \n\ - \ \n \n\n\n" -tracking_camera: - realsense2_camera: - accel_fps: 0 - accel_frame_id: tracking_camera_accel_frame - accel_optical_frame_id: tracking_camera_accel_optical_frame - align_depth: false - aligned_depth_to_color_frame_id: tracking_camera_aligned_depth_to_color_frame - aligned_depth_to_fisheye1_frame_id: tracking_camera_aligned_depth_to_fisheye1_frame - aligned_depth_to_fisheye2_frame_id: tracking_camera_aligned_depth_to_fisheye2_frame - aligned_depth_to_fisheye_frame_id: tracking_camera_aligned_depth_to_fisheye_frame - aligned_depth_to_infra1_frame_id: tracking_camera_aligned_depth_to_infra1_frame - aligned_depth_to_infra2_frame_id: tracking_camera_aligned_depth_to_infra2_frame - allow_no_texture_points: false - base_frame_id: tracking_camera_link - calib_odom_file: /home/robotx/catkin_workspaces/smb_dev/src/core/smb/config/tracking_camera_config.json - clip_distance: -1.0 - color_fps: 30 - color_frame_id: tracking_camera_color_frame - color_height: 480 - color_optical_frame_id: tracking_camera_color_optical_frame - color_width: 640 - confidence_fps: 30 - confidence_height: 480 - confidence_width: 640 - depth_fps: 30 - depth_frame_id: tracking_camera_depth_frame - depth_height: 480 - depth_optical_frame_id: tracking_camera_depth_optical_frame - depth_width: 640 - device_type: t265 - enable_accel: true - enable_color: true - enable_confidence: true - enable_depth: true - enable_fisheye: false - enable_fisheye1: false - enable_fisheye2: false - enable_gyro: true - enable_infra: false - enable_infra1: false - enable_infra2: false - enable_pointcloud: false - enable_pose: true - enable_sync: false - filters: '' - fisheye1_frame_id: tracking_camera_fisheye1_frame - fisheye1_optical_frame_id: tracking_camera_fisheye1_optical_frame - fisheye2_frame_id: tracking_camera_fisheye2_frame - fisheye2_optical_frame_id: tracking_camera_fisheye2_optical_frame - fisheye_fps: 30 - fisheye_frame_id: tracking_camera_fisheye_frame - fisheye_height: 0 - fisheye_optical_frame_id: tracking_camera_fisheye_optical_frame - fisheye_width: 0 - gyro_fps: 0 - gyro_frame_id: tracking_camera_gyro_frame - gyro_optical_frame_id: tracking_camera_gyro_optical_frame - imu_optical_frame_id: tracking_camera_imu_optical_frame - infra1_frame_id: tracking_camera_infra1_frame - infra1_optical_frame_id: tracking_camera_infra1_optical_frame - infra2_frame_id: tracking_camera_infra2_frame - infra2_optical_frame_id: tracking_camera_infra2_optical_frame - infra_fps: 30 - infra_height: 480 - infra_rgb: false - infra_width: 640 - initial_reset: false - json_file_path: '' - linear_accel_cov: 0.01 - odom_frame_id: tracking_camera_odom_frame - ordered_pc: false - pointcloud_texture_index: 0 - pointcloud_texture_stream: RS2_STREAM_COLOR - pose_frame_id: tracking_camera_pose_frame - pose_optical_frame_id: tracking_camera_pose_optical_frame - publish_odom_tf: false - publish_tf: false - reconnect_timeout: 6.0 - rosbag_filename: '' - serial_no: '' - stereo_module: - exposure: - '1': 7500 - '2': 1 - gain: - '1': 16 - '2': 16 - tf_publish_rate: 0.0 - topic_odom_in: /control/smb_diff_drive/odom - unite_imu_method: linear_interpolation - usb_port_id: '' - wait_for_device_timeout: -1.0 - tracking_module: - enable_mapping: false - enable_pose_jumping: false - enable_relocalization: false -versavis_imu_receiver: - imu_acceleration_covariance: 0.043864908 - imu_accelerator_sensitivity: 0.000833 - imu_gyro_covariance: 6e-9 - imu_gyro_sensitivity: 0.04 - imu_pub_topic: /imu - imu_sub_topic: /versavis/imu_micro diff --git a/ros2/examples/smb_estimator_graph_ros2/CMakeLists.txt b/ros2/examples/smb_estimator_graph_ros2/CMakeLists.txt index 1435f97a..0af82919 100644 --- a/ros2/examples/smb_estimator_graph_ros2/CMakeLists.txt +++ b/ros2/examples/smb_estimator_graph_ros2/CMakeLists.txt @@ -17,11 +17,14 @@ find_package(sensor_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) -find_package(graph_msf_ros2 REQUIRED) -find_package(graph_msf REQUIRED) find_package(Glog REQUIRED) -find_package(GTSAM REQUIRED) -find_package(GTSAM_UNSTABLE REQUIRED) +find_package(graph_msf REQUIRED) +find_package(graph_msf_ros2 REQUIRED) + +# Non propagated ros dependencies +find_package(std_srvs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(tf2_eigen REQUIRED) # Display Eigen version and path message("Eigen Version: ${EIGEN3_VERSION_STRING}") @@ -38,11 +41,8 @@ endif() # Define include directories include_directories( include - SYSTEM ${GTSAM_INCLUDE_DIR} - SYSTEM ${GTSAM_UNSTABLE_INCLUDE_DIR} - ${EIGEN3_INCLUDE_DIR} - ${graph_msf_ros2_INCLUDE_DIRS} - ${graph_msf_INCLUDE_DIRS} + ${std_srvs_INCLUDE_DIRS} + ${visualization_msgs_INCLUDE_DIRS} ) # Library @@ -52,15 +52,20 @@ add_library(${PROJECT_NAME} src/lib/SmbStaticTransforms.cpp ) +# Link dependencies target_link_libraries(${PROJECT_NAME} - gtsam - gtsam_unstable - graph_msf::graph_msf - ${graph_msf_ros2_LIBRARIES} + graph_msf_ros2::graph_msf_ros2 ${GLOG_LIBRARIES} ${gflags_LIBRARIES} ) +# Print the include directories for debugging +get_target_property(INC_DIR graph_msf::graph_msf INTERFACE_INCLUDE_DIRECTORIES) +message("Include dir for graph_msf::graph_msf: ${INC_DIR}") +get_target_property(INC_DIR graph_msf_ros2::graph_msf_ros2 INTERFACE_INCLUDE_DIRECTORIES) +message("Include dir for graph_msf_ros2::graph_msf_ros2: ${INC_DIR}") + +# Ament dependencies ament_target_dependencies(${PROJECT_NAME} rclcpp std_msgs @@ -68,23 +73,14 @@ ament_target_dependencies(${PROJECT_NAME} nav_msgs tf2 tf2_ros - graph_msf - graph_msf_ros2 Glog gflags - GTSAM ) # Executable add_executable(${PROJECT_NAME}_node src/smb_estimator_node.cpp) target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME} - gtsam - gtsam_unstable - ${graph_msf_LIBRARIES} - ${graph_msf_ros2_LIBRARIES} - ${GLOG_LIBRARIES} - ${gflags_LIBRARIES} ) ament_target_dependencies(${PROJECT_NAME}_node @@ -94,8 +90,6 @@ ament_target_dependencies(${PROJECT_NAME}_node nav_msgs tf2 tf2_ros - graph_msf - graph_msf_ros2 ) # Add clang tooling diff --git a/ros2/examples/smb_estimator_graph_ros2/config/compslam/compslam_lio_RS16_params.yaml b/ros2/examples/smb_estimator_graph_ros2/config/compslam/compslam_lio_RS16_params.yaml deleted file mode 100644 index 3bfdebc8..00000000 --- a/ros2/examples/smb_estimator_graph_ros2/config/compslam/compslam_lio_RS16_params.yaml +++ /dev/null @@ -1,109 +0,0 @@ -#Common -loamVerbosity: 0 #Debug Output, 0-disable, higher for more verbose output -scanPeriod: 0.1 #Expected scan period for input pointcloud in seconds. Used of Distortion correction -ioRatio: 2 #Ratio of publish rate of LaserOdometry w.r.t input PCL rate. LO is calculated for each PCL but published at slower rate to Mapping, default 2 -lidarFrame: rslidar #LiDAR frame name - used for LiDAR-to-Sensor transform lookup -rotateInputCloud: false #Flag to rotate input cloud before estimating odometry/map so produced resuts are ROS frame aligned irrespective of LiDAR mounting orientation -inputCloudRotation: [0.0, 0.0, 0.0] #Rotation applied to input cloud - ORDER YPR(radians) #90 deg = 1.5707963268 rad - -#External Prior/Transform Input -forceExtPriorUseMapping : false #Flag to force use of FULL external prior instead of LaserOdometry TRANSLATION Only Estimate -extPriorAvailable: true #Flag to check if 'Primary' external prior is available -extOdomFrame: imu #External Prior odometry frame name -extFixedFrame: odom #External Prior fixed frame name -extSensorFrame: imu_link #External Prior sensor frame name -extOdomTimeOffset : 0.0 #Timeoffset (seconds) between LiDAR pointcloud and external source -fallbackExtPriorAvailable: false #Flag to check if 'Fallback ' external prior is available -fallbackExtOdomFrame: vio_imu #Fallback External Prior odometry frame name -fallbackExtFixedFrame: vio_imu_init #Fallback External Prior fixed frame name -fallbackExtSensorFrame: imu_sensor_frame #Fallback External Prior sensor frame name -fallbackExtOdomTimeOffset: 0.00 #Timeoffset (seconds) between LiDAR pointcloud and Fallbackexternal source - -#MultiScanRegistration -lidar: VLP-16 #Choose LiDAR type - options: VLP-16 HDL-32 HDL-64E O1-16 O1-64 Bperl-32 -useCloudIntensityandRingFields : false #Flag to use input pointcloud intensity or ring fields. Converts to custom PointXYZIR instead of PointXYZ. True for Bpearl -uniformLidarRingDistance: true # Whether the LiDAR rings are uniformly distributed in elevation, does not hold for e.g. Helios -ringAngleTolerance: 0.5 # [deg], Tolerance up to which angle rays are considered to belong to same ring, only impact if argument before is false -imuHistorySize: 800 #IMU Message Buffer Size , default: 200 -minRange: 0.1 #Minimum Range of useful points, default: 0.1 -maxRange: 80.0 #Maximum Range of useful points, default: 80 -featureRegions: 6 #Number of Azimuth Regions Pointcloud is divided, default:6 -curvatureRegion: 5 #Number of neigboring points on a scan line on each side of a point used for calculating curvature of that point, default 5 -maxCornerSharp: 2 #Number of Sharp Features per scan line in each curvatureRegion, default:2 -maxCornerLessSharp: 20 #Number of Less Sharp Features per scan line in each curvatureRegion, default:10*maxCornerLessSharp -maxSurfaceFlat: 4 #Number of FlatFeatures per scan line in each curvatureRegion, default:4 -surfaceCurvatureThreshold: 0.1 #Threshold above which feature is categorized Sharp, default 0.1 -lessFlatFilterSize: 0.2 #Leaf Size for downsampling remaing pointcloud after feature selection, default 0.2 -checkInvalidFeatureRegions: false -publishDebugArrowsToRemovedFeatures: false -azimuthBoundaries: [1.1,1.5,1.7,2.2] #ANYmal - -#LaserOdometry -undistortInputCloud: true #If true External Prior or Motion Model will be used for LiDAR Ego Motion Compensation of input cloud -odomMaxIterations: 25 #Maximum Number of LO optimization iterations, default 25 -odomMinIterations: 1 #Minimum Number of LO optimization iterations, default 4, (set 0 to perform only first iteration at minimum) -odomDeltaTAbort: 0.05 #Translation threshold for optimization convergence, deafult 0.1 (m) -odomDeltaRAbort: 0.05 #Rotation threshold for optimization convergence, deafult 0.1 (deg) -odomDegenEigVal: 30 #Minimum eignevalue threshold for determining degeneracy of LO optimization, default 30 -odomRelativeTranslationMax: 0.8 #Max translation threshold between two pointclouds for external odometry to be considered valid input. Determined w.r.t set max robot movement speed -odomRelativeRotationMax: 0.1 #Max rotation threshold between two pointclouds from external odometry to be considered valid input. Determined w.r.t set max robot movement speed - -#LaserMapping -mapMaxIterations: 10 #Maximum Number of LM optimization iterations, default 10 -mapMinIterations: 1 #Minimum Number of LM optimization iterations, default 1, (set 0 to perform only first iteration at minimum) -mapDeltaTAbort: 0.05 #Translation threshold for optimization convergence, deafult 0.05 (m) -mapDeltaRAbort: 0.05 #Rotation threshold for optimization convergence, deafult 0.05 (deg) -cornerFilterSize: 0.2 #Leaf Size for downsampling current CORNER pointcloud before merging in map, default 0.2 -surfaceFilterSize: 0.2 #Leaf Size for downsampling current FLAT/SURFACE pointcloud before merging in map, default 0.2 -mapVisFilterSize: 0.4 #Leaf Size for downsampling current visualization Map pointcloud, default 0.5 -rndSampleMapVisNoOfSamples: 100000 #Number of Samples for random sampling of the visualized map cloud, default 100000 -mapCubeSize: 10.0 #Size of Cube/Voxel used for saving internal map (meters), default 50 -mapDimensionsInCubes: [101,21,101] #WxHxD of internal map in CUBE UNITS, default:[21,11,21] #Width,Height,Depth -mapStartLocationInCubes: [50,10,50] #Robot start position in internal map in CUBE UNITS, default:[10,5,10] #Width,Height,Depth -numNeighborSubmapCubes: 4 #Number of Neigboring cubes in +/- direction along each axis to build submap for matching -mapDegenEigVal: 10 #Minimum eignevalue threshold for determining degeneracy of LM optimization, default 30 -mapPriorNormThresh: 0.5 #Max translation threshold between two pointclouds from external odometry to be considered valid input. Determined w.r.t set max robot movement speed -useSavedSubmapsForInitialization: false #Use saved submap on disc for initialization -submapLocalizationInitGuess: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #Initialization guess of robot location in submap - ORDER: XYZ(meters) - YPR(radians) -waitAndUseExternalSubmapsForInit: false #Wait for external submaps before initlization. Trigger external submaps through service call -submapsRequestServiceCall: "" #Name of service call to request submaps from other node (xyz/share_submaps) -useExternalSubmapOriginForInit: false #Use origin of external submaps as initlization guess for co-localization instead of current external pose (default: false) -forcePathPublishing: true #Force ROS path message publishing for mapping odometry -mapPublishRateDivider: 20 #Publishes full map at 1.0/(scanPeriod * ioRatio * mapPublishRateDivider), default: 20(0.25 Hz) -useExtRotationAlignment: true #Use external fixed & map frame to detemine gravity misalignment, default: true -submapVisCropRange: 0 #Range of cropping for submap visualization (set 0 to disable) - -#Factor Graph -imuFrame: imu_link #Frame of IMU used in integrator - Used for lookup transform with lidarFrame -imuRate: 400 #Rate of IMU input (Hz) used for initialization of gravity, initial Roll/Pitch and gyro bias -imuTimeForInit: 0.5 #Time interval of IMU measurements used for initialization -imuTimeOffset: 0.0 #seconds #Offset between IMU and LiDAR Measurements - Depending on LiDAR timestamp first(+0.05) or last(-0.05) -initGraphRollPitchFromIMU: true #Initialize graph roll/pitch from IMU -globalZaxisPointsUp: true #Z-axis of global coordinate system pointing up -accNoiseDensity: 7.84e-04 #Continuous-time "Covariance" of accelerometer -accBiasRandomWalk: 1.0e-04 #Continuous-time "Covariance" describing accelerometer bias random walk (biasAccCovariance) -gyrNoiseDensity: 3.4906585e-5 #Continuous-time "Covariance" of gyroscope measurements -gyrBiasRandomWalk: 1.0e-04 #Continuous-time "Covariance" describing gyroscope bias random walk (biasOmegaCovariance) -imuIntegrationCovariance: 1.0e-08 #Continuous-time "Covariance" describing integration uncertainty -imuBiasAccOmegaInt: 1.0 #Covariance of bias used for pre-integration -accBiasPrior: [0.0, 0.0, 0.0] #Prior/starting value of accelerometer bias -gyrBiasPrior: [0.0, 0.0, 0.0] #Prior/starting value of gyroscope bias -smootherLag: 3.0 #Lag of fixed lag smoother[seconds] -additonalIterations: 3 #Additional iterations of graph optimizer after update with new factors -positionReLinTh: 0.05 #Position linearization threshold -rotationReLinTh: 0.05 #Rotation linearization threshold -velocityReLinTh: 0.1 #Linear Velocity linearization threshold -accBiasReLinTh: 0.1 #Accelerometer bias linearization threshold -gyrBiasReLinTh: 0.1 #Gyroscope bias linearization threshold -relinearizeSkip: 1 -enableRelinearization: true -evaluateNonlinearError: false -cacheLinearizedFactors: true -findUnusedFactorSlots: false -enablePartialRelinearizationCheck: true -enableDetailedResults: false -poseBetweenNoise: [0.5, 0.5, 0.05, 0.1, 0.1, 0.1] #Noise of add between factor -ORDER RPY(rad) - XYZ(meters) -zeroMotionDetection: false #Detect and Add Zero Motion Factors(Zero delta Pose and Velocity) -zeroMotionThreshold: 0.01 #Zero motion threshold in meters (Currently only motion detection is translation only) -minZeroMoitionDetections: 10 #Minimum number of consective zero motions detected before factors are added -gravityRollPitchFactors: false #Add Gravity-aligned Roll-Pitch from IMU when in Zero motion (only works if Zero-Motion Factors are added) diff --git a/ros2/examples/smb_estimator_graph_ros2/config/core/core_graph_params.yaml b/ros2/examples/smb_estimator_graph_ros2/config/core/core_graph_params.yaml index 0ded6873..fac5c153 100644 --- a/ros2/examples/smb_estimator_graph_ros2/config/core/core_graph_params.yaml +++ b/ros2/examples/smb_estimator_graph_ros2/config/core/core_graph_params.yaml @@ -16,7 +16,7 @@ # Factor Graph graph_params: - realTimeSmootherLag: 2.0 # Lag of real-time fixed lag smoother[seconds] + realTimeSmootherLag: 10.0 # Lag of real-time fixed lag smoother[seconds] realTimeSmootherUseIsam: true realTimeSmootherUseCholeskyFactorization: true # CHOLESKY faster, QR numerically more stable useAdditionalSlowBatchSmoother: true # If true, a slower smoother is used in addition to the real-time smoother diff --git a/ros2/examples/smb_estimator_graph_ros2/config/dumped_params/2023-06-13-13-15-57.yaml b/ros2/examples/smb_estimator_graph_ros2/config/dumped_params/2023-06-13-13-15-57.yaml deleted file mode 100644 index 403a3779..00000000 --- a/ros2/examples/smb_estimator_graph_ros2/config/dumped_params/2023-06-13-13-15-57.yaml +++ /dev/null @@ -1,523 +0,0 @@ -control: - joint_state_controller: - publish_rate: 50 - type: joint_state_controller/JointStateController - joy_teleop: - joy_node: - autorepeat_rate: 20 - deadzone: 0.1 - dev: /dev/input/js0 - teleop_twist_joy: - axis_angular: 0 - axis_linear: 1 - enable_button: 4 - enable_turbo_button: 5 - scale_angular: 1.2 - scale_angular_turbo: 1.2 - scale_linear: 1.6 - scale_linear_turbo: 1.0 - lowlevel_controller: - LF_WHEEL_JOINT: - has_acceleration_limits: true - has_effort_limits: true - has_jerk_limits: true - has_position_limits: false - has_velocity_limits: true - max_acceleration: 5.0 - max_effort: 1.0 - max_jerk: 100.0 - max_velocity: 2.0 - LF_WHEEL_JOINT_dc_controller: - antiwindup: true - d: 0.0 - i: 150.0 - i_clamp: 300 - i_clamp_max: 300.0 - i_clamp_min: -300.0 - p: 50.0 - publish_state: true - LF_WHEEL_velocity_controller: - joint: LF_WHEEL_JOINT - pid: - d: 10.0 - i: 0.01 - p: 100.0 - type: velocity_controllers/JointVelocityController - RF_WHEEL_JOINT: - has_acceleration_limits: true - has_effort_limits: true - has_jerk_limits: true - has_position_limits: false - has_velocity_limits: true - max_acceleration: 5.0 - max_effort: 1.0 - max_jerk: 100.0 - max_velocity: 2.0 - RF_WHEEL_JOINT_dc_controller: - antiwindup: true - d: 0.0 - i: 150.0 - i_clamp: 300 - i_clamp_max: 300.0 - i_clamp_min: -300.0 - p: 50.0 - publish_state: true - RF_WHEEL_velocity_controller: - joint: RF_WHEEL_JOINT - pid: - d: 10.0 - i: 0.01 - p: 100.0 - type: velocity_controllers/JointVelocityController - WHEEL_JOINT_ff_param: - ff_general: 25 - ff_pure_rotation: 100 - robot_joint_publisher: - publish_rate: 50 - type: joint_state_controller/JointStateController - smb: - ang_vel_scale: 1.8 - hardware_interface: - joints: - - LF_WHEEL_JOINT - - RF_WHEEL_JOINT - loop_hz: 400 - lin_vel_scale: 1.8 - smb_diff_drive: - angular: - z: - has_acceleration_limits: true - has_velocity_limits: true - max_acceleration: 5.0 - max_velocity: 2.0 - base_frame_id: base_link - cmd_vel_timeout: 0.25 - enable_odom_tf: false - estimate_velocity_from_position: false - left_wheel: - - LF_WHEEL_JOINT - left_wheel_radius_multiplier: 1.0 - linear: - x: - has_acceleration_limits: true - has_velocity_limits: true - max_acceleration: 5.0 - max_velocity: 2.0 - pose_covariance_diagonal: - - 0.001 - - 0.001 - - 0.001 - - 0.001 - - 0.001 - - 0.03 - publish_rate: 50.0 - right_wheel: - - RF_WHEEL_JOINT - right_wheel_radius_multiplier: 1.0 - twist_covariance_diagonal: - - 0.001 - - 0.001 - - 0.001 - - 0.001 - - 0.001 - - 0.03 - type: diff_drive_controller/DiffDriveController - velocity_rolling_window_size: 2 - wheel_radius_multiplier: 1.0 - wheel_separation_multiplier: 1.0 - smb_lowlevel_controller: - command_smb: true - control_namespace: /control - controller_namespace: lowlevel_controller - port: /dev/ttyUSB0 - smb: - ang_vel_scale: 1.8 - hardware_interface: - joints: - - LF_WHEEL_JOINT - - RF_WHEEL_JOINT - loop_hz: 400 - lin_vel_scale: 1.8 - smb_robot_state_publisher: - publish_frequency: 50 - use_tf_static: true - twist_mux: - locks: - - name: e_stop - priority: 255 - timeout: 0.0 - topic: joy_teleop/e_stop - - name: e_stop_opc - priority: 200 - timeout: 0.0 - topic: opc/e_stop - topics: - - name: RC - priority: 15 - timeout: 0.5 - topic: smb_lowlevel_controller/rc_twist - - name: joy - priority: 10 - timeout: 0.5 - topic: joy_teleop/cmd_vel - - name: keyboard - priority: 9 - timeout: 0.5 - topic: keyboard_teleop/cmd_vel - - name: joy_opc - priority: 8 - timeout: 0.5 - topic: opc/cmd_vel - - name: mpc_command_twist - priority: 5 - timeout: 0.2 - topic: smb_mpc/command_twist - - name: teb_planner_twist - priority: 3 - timeout: 1.0 - topic: teb_planner_twist -odometry: - tracking_camera_odometry_conversion: - in_odom_frame: tracking_camera_pose_frame - in_sensor_frame: tracking_camera_pose_frame - out_odom_frame: tracking_camera_odom - out_sensor_frame: base_link -rosdistro: 'noetic - - ' -roslaunch: - uris: - host_smb_263_nuc__35755: http://smb-263-nuc:35755/ -rosserial_python: - baud: 250000 - port: /dev/versavis -rosversion: '1.16.0 - - ' -run_id: d353b820-09d9-11ee-9f1a-13a7083ee06f -serial_node: - baud: 115200 - port: /dev/smb-power -simulation: false -smb_description: "\n\n\n\n\n\n \n \n\ - \ \n \n \n \n \n \n \n \n\ - \ \n \n\ - \ \n \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \ - \ \n \n \n \n \n \n \n \n \n \n \n \n \n \n\ - \ Gazebo/Grey\n \n \n Gazebo/Red\n \n \n \n \n\ - \ \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \ - \ \n \n \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n 10000000.0\n\ - \ 1.0\n 0.005\n 100.0\n\ - \ Gazebo/DarkGrey\n \n \n transmission_interface/SimpleTransmission\n\ - \ \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ \n \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ 1\n \n \n\ - \ \n \n \n \n \n\ - \ \n \n \n \n \ - \ \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n 10000000.0\n\ - \ 1.0\n 0.005\n 100.0\n\ - \ Gazebo/DarkGrey\n \n \n transmission_interface/SimpleTransmission\n\ - \ \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ \n \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ 1\n \n \n\ - \ \n \n \n \n \n\ - \ \n \n \n \n \ - \ \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n 10000000.0\n\ - \ 1.0\n 0.005\n 100.0\n\ - \ Gazebo/DarkGrey\n \n \n transmission_interface/SimpleTransmission\n\ - \ \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ \n \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ 1\n \n \n\ - \ \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n 10000000.0\n\ - \ 1.0\n 0.005\n 100.0\n\ - \ Gazebo/DarkGrey\n \n \n transmission_interface/SimpleTransmission\n\ - \ \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ \n \n hardware_interface/VelocityJointInterface\n\ - \ hardware_interface/EffortJointInterface\n\ - \ hardware_interface/PositionJointInterface\n\ - \ 1\n \n \n\ - \ \n \n \n \n \n \n \n\ - \ \n \n \n \ - \ \n \n \n \ - \ \n \n \n \ - \ \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n \n \n Gazebo/Orange\n \n \n true\n \n\ - \ \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \ - \ \n \n \n \n \n \n \n \n Gazebo/Red\n \n \n \n \n \n \n \n \n true\n\ - \ \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n \n\ - \ \n \n \n Gazebo/Grey\n\ - \ \n \n \n \n \n \n \n \n\ - \ \n \n \n \n\ - \ \n \n \n \ - \ \n \n \n \n \n \ - \ \n \n \n \n \n\ - \ \n \n \n \n \n \n \n \n \n\ - \ \n \n \n \n \n \n \ - \ \n \n \n \n \n \ - \ \n \n \n Gazebo/Black\n\ - \ \n \n \n \n \n \n \n \n true\n\ - \ \n \n \n \n \n \n \n\ - \ \n \n\n\n" -tracking_camera: - realsense2_camera: - accel_fps: 0 - accel_frame_id: tracking_camera_accel_frame - accel_optical_frame_id: tracking_camera_accel_optical_frame - align_depth: false - aligned_depth_to_color_frame_id: tracking_camera_aligned_depth_to_color_frame - aligned_depth_to_fisheye1_frame_id: tracking_camera_aligned_depth_to_fisheye1_frame - aligned_depth_to_fisheye2_frame_id: tracking_camera_aligned_depth_to_fisheye2_frame - aligned_depth_to_fisheye_frame_id: tracking_camera_aligned_depth_to_fisheye_frame - aligned_depth_to_infra1_frame_id: tracking_camera_aligned_depth_to_infra1_frame - aligned_depth_to_infra2_frame_id: tracking_camera_aligned_depth_to_infra2_frame - allow_no_texture_points: false - base_frame_id: tracking_camera_link - calib_odom_file: /home/robotx/catkin_workspaces/smb_dev/src/core/smb/config/tracking_camera_config.json - clip_distance: -1.0 - color_fps: 30 - color_frame_id: tracking_camera_color_frame - color_height: 480 - color_optical_frame_id: tracking_camera_color_optical_frame - color_width: 640 - confidence_fps: 30 - confidence_height: 480 - confidence_width: 640 - depth_fps: 30 - depth_frame_id: tracking_camera_depth_frame - depth_height: 480 - depth_optical_frame_id: tracking_camera_depth_optical_frame - depth_width: 640 - device_type: t265 - enable_accel: true - enable_color: true - enable_confidence: true - enable_depth: true - enable_fisheye: false - enable_fisheye1: false - enable_fisheye2: false - enable_gyro: true - enable_infra: false - enable_infra1: false - enable_infra2: false - enable_pointcloud: false - enable_pose: true - enable_sync: false - filters: '' - fisheye1_frame_id: tracking_camera_fisheye1_frame - fisheye1_optical_frame_id: tracking_camera_fisheye1_optical_frame - fisheye2_frame_id: tracking_camera_fisheye2_frame - fisheye2_optical_frame_id: tracking_camera_fisheye2_optical_frame - fisheye_fps: 30 - fisheye_frame_id: tracking_camera_fisheye_frame - fisheye_height: 0 - fisheye_optical_frame_id: tracking_camera_fisheye_optical_frame - fisheye_width: 0 - gyro_fps: 0 - gyro_frame_id: tracking_camera_gyro_frame - gyro_optical_frame_id: tracking_camera_gyro_optical_frame - imu_optical_frame_id: tracking_camera_imu_optical_frame - infra1_frame_id: tracking_camera_infra1_frame - infra1_optical_frame_id: tracking_camera_infra1_optical_frame - infra2_frame_id: tracking_camera_infra2_frame - infra2_optical_frame_id: tracking_camera_infra2_optical_frame - infra_fps: 30 - infra_height: 480 - infra_rgb: false - infra_width: 640 - initial_reset: false - json_file_path: '' - linear_accel_cov: 0.01 - odom_frame_id: tracking_camera_odom_frame - ordered_pc: false - pointcloud_texture_index: 0 - pointcloud_texture_stream: RS2_STREAM_COLOR - pose_frame_id: tracking_camera_pose_frame - pose_optical_frame_id: tracking_camera_pose_optical_frame - publish_odom_tf: false - publish_tf: false - reconnect_timeout: 6.0 - rosbag_filename: '' - serial_no: '' - stereo_module: - exposure: - '1': 7500 - '2': 1 - gain: - '1': 16 - '2': 16 - tf_publish_rate: 0.0 - topic_odom_in: /control/smb_diff_drive/odom - unite_imu_method: linear_interpolation - usb_port_id: '' - wait_for_device_timeout: -1.0 - tracking_module: - enable_mapping: false - enable_pose_jumping: false - enable_relocalization: false -versavis_imu_receiver: - imu_acceleration_covariance: 0.043864908 - imu_accelerator_sensitivity: 0.000833 - imu_gyro_covariance: 6e-9 - imu_gyro_sensitivity: 0.04 - imu_pub_topic: /imu - imu_sub_topic: /versavis/imu_micro diff --git a/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml b/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml index a816777f..8452aee8 100644 --- a/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml +++ b/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml @@ -8,7 +8,7 @@ useWheelLinearVelocities: false useVioOdometry: false ## Rates - lioOdometryRate: 10 + lioOdometryRate: 100 wheelOdometryBetweenRate: 50 wheelLinearVelocitiesRate: 50 vioOdometryRate: 50 diff --git a/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h b/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h index 71c508c3..eed44167 100644 --- a/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h +++ b/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h @@ -32,6 +32,12 @@ class SmbStaticTransforms : public graph_msf::StaticTransformsTf { const std::string& getWheelLinearVelocityRightFrame() const { return wheelLinearVelocityRightFrame_; } const std::string& getVioOdometryFrame() const { return vioOdometryFrame_; } + // Set flags + void setUseLioOdometryFlag(bool flag) { useLioOdometryFlag_ = flag; } + void setUseVioOdometryFlag(bool flag) { useVioOdometryFlag_ = flag; } + void setUseWheelOdometryBetweenFlag(bool flag) { useWheelOdometryBetweenFlag_ = flag; } + void setUseWheelLinearVelocitiesFlag(bool flag) { useWheelLinearVelocitiesFlag_ = flag; } + private: void findTransformations() override; @@ -41,6 +47,12 @@ class SmbStaticTransforms : public graph_msf::StaticTransformsTf { std::string wheelLinearVelocityLeftFrame_; std::string wheelLinearVelocityRightFrame_; std::string vioOdometryFrame_; + + // Odometry flags + bool useLioOdometryFlag_ = false; + bool useVioOdometryFlag_ = false; + bool useWheelOdometryBetweenFlag_ = false; + bool useWheelLinearVelocitiesFlag_ = false; }; } // namespace smb_se diff --git a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_sim.launch.py b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_sim.launch.py index 9ccbd6ed..5ebef284 100644 --- a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_sim.launch.py +++ b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_sim.launch.py @@ -32,7 +32,7 @@ def generate_launch_description(): # Declare launch arguments DeclareLaunchArgument( 'use_sim_time', - default_value='false', + default_value='true', description='Use simulation time' ), DeclareLaunchArgument( diff --git a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp index cd0dde7d..816b9917 100644 --- a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp +++ b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp @@ -23,6 +23,16 @@ SmbEstimator::SmbEstimator(std::shared_ptr& node) : graph_msf::Gra REGULAR_COUT << GREEN_START << " SmbEstimator-Constructor called." << COLOR_END << std::endl; staticTransformsPtr_ = std::make_shared(node_); + + // Set the odometry flags in SmbStaticTransforms + auto smbStaticTransforms = std::dynamic_pointer_cast(staticTransformsPtr_); + if (smbStaticTransforms) { + smbStaticTransforms->setUseLioOdometryFlag(useLioOdometryFlag_); + smbStaticTransforms->setUseVioOdometryFlag(useVioOdometryFlag_); + smbStaticTransforms->setUseWheelOdometryBetweenFlag(useWheelOdometryBetweenFlag_); + smbStaticTransforms->setUseWheelLinearVelocitiesFlag(useWheelLinearVelocitiesFlag_); + } + // Call setup after declaring parameters SmbEstimator::setup(); } diff --git a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp index 07602e88..5f2c6be5 100644 --- a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp +++ b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp @@ -35,55 +35,62 @@ void SmbStaticTransforms::findTransformations() { // Temporary variable geometry_msgs::msg::TransformStamped transform; + Eigen::Isometry3d eigenTransform; // Imu to LiDAR Link --- - REGULAR_COUT << COLOR_END << " Waiting for transform LO for 10 seconds." << std::endl; - transform = tf_buffer_->lookupTransform(imuFrame_, lidarOdometryFrame_, tf2::TimePointZero, tf2::durationFromSec(1.0)); - Eigen::Isometry3d eigenTransform = tf2::transformToEigen(transform.transform); - lv_T_frame1_frame2(imuFrame_, lidarOdometryFrame_) = eigenTransform; + if (useLioOdometryFlag_) { + REGULAR_COUT << COLOR_END << " Waiting for transform LO for 10 seconds." << std::endl; + transform = tf_buffer_->lookupTransform(imuFrame_, lidarOdometryFrame_, tf2::TimePointZero, tf2::durationFromSec(1.0)); + eigenTransform = tf2::transformToEigen(transform.transform); + lv_T_frame1_frame2(imuFrame_, lidarOdometryFrame_) = eigenTransform; - std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END << " Translation I_Lidar: " << imuFrame_ << " " << lidarOdometryFrame_ - << " " << rv_T_frame1_frame2(imuFrame_, lidarOdometryFrame_).translation() << std::endl; - lv_T_frame1_frame2(lidarOdometryFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, lidarOdometryFrame_).inverse(); + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END << " Translation I_Lidar: " << imuFrame_ << " " << lidarOdometryFrame_ + << " " << rv_T_frame1_frame2(imuFrame_, lidarOdometryFrame_).translation() << std::endl; + lv_T_frame1_frame2(lidarOdometryFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, lidarOdometryFrame_).inverse(); + } // Imu to VIO Link --- - REGULAR_COUT << COLOR_END << " Waiting for transform VIO for 10 seconds." << std::endl; - transform = tf_buffer_->lookupTransform(imuFrame_, vioOdometryFrame_, tf2::TimePointZero, tf2::durationFromSec(1.0)); - eigenTransform = tf2::transformToEigen(transform.transform); - lv_T_frame1_frame2(imuFrame_, vioOdometryFrame_) = eigenTransform; + if (useVioOdometryFlag_) { + REGULAR_COUT << COLOR_END << " Waiting for transform VIO for 10 seconds." << std::endl; + transform = tf_buffer_->lookupTransform(imuFrame_, vioOdometryFrame_, tf2::TimePointZero, tf2::durationFromSec(1.0)); + eigenTransform = tf2::transformToEigen(transform.transform); + lv_T_frame1_frame2(imuFrame_, vioOdometryFrame_) = eigenTransform; - std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END << " Translation I_VIO: " << imuFrame_ << " " << vioOdometryFrame_ << " " - << rv_T_frame1_frame2(imuFrame_, vioOdometryFrame_).translation() << std::endl; - lv_T_frame1_frame2(vioOdometryFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, vioOdometryFrame_).inverse(); + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END << " Translation I_VIO: " << imuFrame_ << " " << vioOdometryFrame_ << " " + << rv_T_frame1_frame2(imuFrame_, vioOdometryFrame_).translation() << std::endl; + lv_T_frame1_frame2(vioOdometryFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, vioOdometryFrame_).inverse(); + } // Wheel Frames --- - REGULAR_COUT << RED_START - << " As the Wheels are turning, we only use the position of the wheels frames and use the orientation of the baseLinkFrame_ frame." - << COLOR_END << std::endl; - - // Left Wheel - REGULAR_COUT << COLOR_END << " Waiting for transform for 10 seconds." << std::endl; - transform = tf_buffer_->lookupTransform(imuFrame_, wheelLinearVelocityLeftFrame_, tf2::TimePointZero, tf2::durationFromSec(1.0)); - Eigen::Isometry3d T_I_WheelLeft = tf2::transformToEigen(transform.transform); - T_I_WheelLeft.matrix().block<3, 3>(0, 0) = rv_T_frame1_frame2(imuFrame_, baseLinkFrame_).matrix().block<3, 3>(0, 0); - lv_T_frame1_frame2(imuFrame_, wheelLinearVelocityLeftFrame_) = T_I_WheelLeft; - - std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END << " Translation I_WheelLeft: " - << rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityLeftFrame_).translation() - << ", Rotation: " << T_I_WheelLeft.matrix().block<3, 3>(0, 0) << std::endl; - lv_T_frame1_frame2(wheelLinearVelocityLeftFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityLeftFrame_).inverse(); - - // Right Wheel - REGULAR_COUT << COLOR_END << " Waiting for transform for 10 seconds." << std::endl; - transform = tf_buffer_->lookupTransform(imuFrame_, wheelLinearVelocityRightFrame_, tf2::TimePointZero, tf2::durationFromSec(1.0)); - Eigen::Isometry3d T_I_WheelRight = tf2::transformToEigen(transform.transform); - T_I_WheelRight.matrix().block<3, 3>(0, 0) = rv_T_frame1_frame2(imuFrame_, baseLinkFrame_).matrix().block<3, 3>(0, 0); - lv_T_frame1_frame2(imuFrame_, wheelLinearVelocityRightFrame_) = T_I_WheelRight; - - std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END << " Translation I_WheelRight: " - << rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityRightFrame_).translation() - << ", Rotation: " << T_I_WheelRight.matrix().block<3, 3>(0, 0) << std::endl; - lv_T_frame1_frame2(wheelLinearVelocityRightFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityRightFrame_).inverse(); + if (useWheelOdometryBetweenFlag_ || useWheelLinearVelocitiesFlag_) { + REGULAR_COUT << RED_START + << " As the Wheels are turning, we only use the position of the wheels frames and use the orientation of the baseLinkFrame_ frame." + << COLOR_END << std::endl; + + // Left Wheel + REGULAR_COUT << COLOR_END << " Waiting for transform for 10 seconds." << std::endl; + transform = tf_buffer_->lookupTransform(imuFrame_, wheelLinearVelocityLeftFrame_, tf2::TimePointZero, tf2::durationFromSec(1.0)); + Eigen::Isometry3d T_I_WheelLeft = tf2::transformToEigen(transform.transform); + T_I_WheelLeft.matrix().block<3, 3>(0, 0) = rv_T_frame1_frame2(imuFrame_, baseLinkFrame_).matrix().block<3, 3>(0, 0); + lv_T_frame1_frame2(imuFrame_, wheelLinearVelocityLeftFrame_) = T_I_WheelLeft; + + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END << " Translation I_WheelLeft: " + << rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityLeftFrame_).translation() + << ", Rotation: " << T_I_WheelLeft.matrix().block<3, 3>(0, 0) << std::endl; + lv_T_frame1_frame2(wheelLinearVelocityLeftFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityLeftFrame_).inverse(); + + // Right Wheel + REGULAR_COUT << COLOR_END << " Waiting for transform for 10 seconds." << std::endl; + transform = tf_buffer_->lookupTransform(imuFrame_, wheelLinearVelocityRightFrame_, tf2::TimePointZero, tf2::durationFromSec(1.0)); + Eigen::Isometry3d T_I_WheelRight = tf2::transformToEigen(transform.transform); + T_I_WheelRight.matrix().block<3, 3>(0, 0) = rv_T_frame1_frame2(imuFrame_, baseLinkFrame_).matrix().block<3, 3>(0, 0); + lv_T_frame1_frame2(imuFrame_, wheelLinearVelocityRightFrame_) = T_I_WheelRight; + + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END << " Translation I_WheelRight: " + << rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityRightFrame_).translation() + << ", Rotation: " << T_I_WheelRight.matrix().block<3, 3>(0, 0) << std::endl; + lv_T_frame1_frame2(wheelLinearVelocityRightFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityRightFrame_).inverse(); + } REGULAR_COUT << GREEN_START << " Transforms looked up successfully." << COLOR_END << std::endl; } diff --git a/ros2/graph_msf_ros2/CMakeLists.txt b/ros2/graph_msf_ros2/CMakeLists.txt index 3bc8a1c7..be64a6bf 100644 --- a/ros2/graph_msf_ros2/CMakeLists.txt +++ b/ros2/graph_msf_ros2/CMakeLists.txt @@ -14,18 +14,14 @@ find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(urdf REQUIRED) -find_package(kdl_parser REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(tf2_eigen REQUIRED) find_package(std_srvs REQUIRED) find_package(visualization_msgs REQUIRED) -find_package(graph_msf REQUIRED) -# Show path of graph_msf -if (graph_msf_FOUND) - message(STATUS "graph_msf was found!") -else() - message(FATAL_ERROR "graph_msf NOT found!") +# Check if the target from graph_msf is already defined +if(NOT TARGET graph_msf::graph_msf) + find_package(graph_msf REQUIRED) endif() # Color (Optional) @@ -36,86 +32,125 @@ if (NOT WIN32) set(Magenta "${Esc}[35m") endif () +########### +## Build ## +########### + # Include directories include_directories( include - ${EIGEN3_INCLUDE_DIR} ) # Library add_library(${PROJECT_NAME} SHARED src/lib/StaticTransformsTf.cpp - src/lib/StaticTransformsUrdf.cpp src/lib/GraphMsfRos2.cpp src/lib/readParams.cpp src/lib/conversions.cpp ) +# Exported alias target +add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME}) # Link dependencies ament_target_dependencies(${PROJECT_NAME} - rclcpp - geometry_msgs - nav_msgs - sensor_msgs - std_msgs - tf2 - tf2_ros - urdf - kdl_parser - tf2_eigen - std_srvs - visualization_msgs - graph_msf - rosidl_default_runtime + PUBLIC + geometry_msgs + nav_msgs + sensor_msgs + std_msgs + std_srvs + visualization_msgs + rosidl_default_runtime ) target_link_libraries(${PROJECT_NAME} - graph_msf::graph_msf + PUBLIC + Eigen3::Eigen + rclcpp::rclcpp + tf2::tf2 + tf2_ros::tf2_ros + tf2_eigen::tf2_eigen + graph_msf::graph_msf ) + +# Include directories for the library +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ +) + # Print the include directories for debugging get_target_property(INC_DIR graph_msf::graph_msf INTERFACE_INCLUDE_DIRECTORIES) message("Include dir for graph_msf::graph_msf: ${INC_DIR}") # Python Scripts ---------------------------------------------------------------- install(PROGRAMS - graph_msf_ros2_py/replay/manual_pose_files_to_tf_and_odom_bag.py - graph_msf_ros2_py/plotting/plot_latest_quantitites_in_folder.py - graph_msf_ros2_py/bag_filter/remove_tf_from_bag.py + ${PROJECT_NAME}_py/replay/manual_pose_files_to_tf_and_odom_bag.py + ${PROJECT_NAME}_py/plotting/plot_latest_quantitites_in_folder.py + ${PROJECT_NAME}_py/bag_filter/remove_tf_from_bag.py DESTINATION lib/${PROJECT_NAME} ) -# Install headers, config, and other files -------------------------------------- -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include/${PROJECT_NAME} +############# +## Install ## +############# + +# Export the include directories and linked libraries +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}Targets # Export the target for downstream usage + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# Install the include directory +install(DIRECTORY include/ + DESTINATION include ) -install(DIRECTORY config rviz - DESTINATION share/${PROJECT_NAME} + +# Install the export file for downstream projects +install(EXPORT ${PROJECT_NAME}Targets + FILE ${PROJECT_NAME}Targets.cmake + NAMESPACE ${PROJECT_NAME}:: + DESTINATION share/${PROJECT_NAME}/cmake +) + +# Version +include(CMakePackageConfigHelpers) +write_basic_package_version_file( + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" + VERSION 1.0.0 + COMPATIBILITY SameMajorVersion +) + +# Configure +configure_package_config_file( + "${PROJECT_NAME}Config.cmake.in" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + INSTALL_DESTINATION share/${PROJECT_NAME}/cmake ) -install(TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME}Targets - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin +# Files +install(FILES + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" + DESTINATION share/${PROJECT_NAME}/cmake ) -# Export package +# Export ament package ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) ament_export_dependencies( - rclcpp geometry_msgs nav_msgs sensor_msgs std_msgs tf2 tf2_ros - urdf - kdl_parser tf2_eigen std_srvs visualization_msgs - graph_msf - Eigen3 rosidl_default_runtime ) diff --git a/ros2/graph_msf_ros2/graph_msf_ros2Config.cmake.in b/ros2/graph_msf_ros2/graph_msf_ros2Config.cmake.in new file mode 100644 index 00000000..e5bd51d2 --- /dev/null +++ b/ros2/graph_msf_ros2/graph_msf_ros2Config.cmake.in @@ -0,0 +1,9 @@ +@PACKAGE_INIT@ + +include("${CMAKE_CURRENT_LIST_DIR}/graph_msf_ros2Targets.cmake") + +# Optionally set legacy-style variables (only if needed) +# set(graph_msf_INCLUDE_DIRS "@CMAKE_INSTALL_INCLUDEDIR@") + +# Set up version compatibility (optional if you use a version file) +# include("${CMAKE_CURRENT_LIST_DIR}/graph_msfConfigVersion.cmake") \ No newline at end of file diff --git a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp index 75b39bab..ac803638 100644 --- a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp +++ b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp @@ -179,7 +179,7 @@ void GraphMsfRos2::initializeSubscribers() { // Imu subImu_ = node_->create_subscription( - "/imu_broadcaster/imu_bike_base_front_top", rclcpp::QoS(ROS_QUEUE_SIZE).best_effort(), + "/imu_topic", rclcpp::QoS(ROS_QUEUE_SIZE).best_effort(), std::bind(&GraphMsfRos2::imuCallback, this, std::placeholders::_1)); RCLCPP_INFO(node_->get_logger(), "GraphMsfRos2 Initialized main IMU subscriber with topic: %s", From c8c118a7e823277858c6797650621758d24ac603 Mon Sep 17 00:00:00 2001 From: Julian Date: Wed, 11 Jun 2025 14:41:01 +0200 Subject: [PATCH 37/60] new ros2 setup --- .../smb_estimator_graph_replay.launch.py | 107 ++++++-------- .../rviz/rviz_config.rviz | 133 ++++++++++++++++++ 2 files changed, 179 insertions(+), 61 deletions(-) create mode 100644 ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz diff --git a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py index d4a357c7..7ba0f39f 100644 --- a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py +++ b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py @@ -6,72 +6,57 @@ from ament_index_python.packages import get_package_share_directory import os + def generate_launch_description(): # Package name - pkg_name = 'smb_estimator_graph_ros2' - + pkg_name = "smb_estimator_graph_ros2" + # Get package share directory pkg_dir = get_package_share_directory(pkg_name) - + # Declare launch arguments - use_sim_time = LaunchConfiguration('use_sim_time') - imu_topic_name = LaunchConfiguration('imu_topic_name') - lidar_odometry_topic_name = LaunchConfiguration('lidar_odometry_topic_name') - core_graph_param_file = LaunchConfiguration('core_graph_param_file') - smb_graph_param_file = LaunchConfiguration('smb_graph_param_file') + use_sim_time = LaunchConfiguration("use_sim_time") + imu_topic_name = LaunchConfiguration("imu_topic_name") + lidar_odometry_topic_name = LaunchConfiguration("lidar_odometry_topic_name") + core_graph_param_file = LaunchConfiguration("core_graph_param_file") + smb_graph_param_file = LaunchConfiguration("smb_graph_param_file") # Default config file paths - default_core_graph_param_file = os.path.join(pkg_dir, 'config', 'core', 'core_graph_params.yaml') - default_smb_graph_param_file = os.path.join(pkg_dir, 'config', 'smb_specific', 'smb_graph_params.yaml') - - return LaunchDescription([ - # Declare launch arguments - DeclareLaunchArgument( - 'use_sim_time', - default_value='true', - description='Use simulation time' - ), - DeclareLaunchArgument( - 'imu_topic_name', - default_value='/imu', - description='IMU topic name' - ), - DeclareLaunchArgument( - 'lidar_odometry_topic_name', - default_value='/mapping/scan2map_odometry', - description='Lidar odometry topic name' - ), - DeclareLaunchArgument( - 'core_graph_param_file', - default_value=default_core_graph_param_file, - description='Core graph parameter file' - ), - DeclareLaunchArgument( - 'smb_graph_param_file', - default_value=default_smb_graph_param_file, - description='SMB graph parameter file' - ), - - # Include the main launch file - IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - os.path.join(pkg_dir, 'launch', 'smb_estimator_graph.launch.py') - ]), - launch_arguments={ - 'use_sim_time': use_sim_time, - 'imu_topic_name': imu_topic_name, - 'lidar_odometry_topic_name': lidar_odometry_topic_name, - 'core_graph_param_file': core_graph_param_file, - 'smb_graph_param_file': smb_graph_param_file - }.items() - ), + default_core_graph_param_file = os.path.join(pkg_dir, "config", "core", "core_graph_params.yaml") + default_smb_graph_param_file = os.path.join(pkg_dir, "config", "smb_specific", "smb_graph_params.yaml") - # RViz Node - Node( - package='rviz2', - executable='rviz2', - name='rviz2', - arguments=['-d', os.path.join(pkg_dir, 'rviz', 'lidar_estimation.rviz')], - output='screen' - ) - ]) \ No newline at end of file + return LaunchDescription( + [ + # Declare launch arguments + DeclareLaunchArgument("use_sim_time", default_value="true", description="Use simulation time"), + DeclareLaunchArgument("imu_topic_name", default_value="/imu", description="IMU topic name"), + DeclareLaunchArgument( + "lidar_odometry_topic_name", default_value="/mapping/scan2map_odometry", description="Lidar odometry topic name" + ), + DeclareLaunchArgument( + "core_graph_param_file", default_value=default_core_graph_param_file, description="Core graph parameter file" + ), + DeclareLaunchArgument( + "smb_graph_param_file", default_value=default_smb_graph_param_file, description="SMB graph parameter file" + ), + # Include the main launch file + IncludeLaunchDescription( + PythonLaunchDescriptionSource([os.path.join(pkg_dir, "launch", "smb_estimator_graph.launch.py")]), + launch_arguments={ + "use_sim_time": use_sim_time, + "imu_topic_name": imu_topic_name, + "lidar_odometry_topic_name": lidar_odometry_topic_name, + "core_graph_param_file": core_graph_param_file, + "smb_graph_param_file": smb_graph_param_file, + }.items(), + ), + # RViz Node + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", os.path.join(pkg_dir, "rviz", "rviz_config.rviz")], + output="screen", + ), + ] + ) diff --git a/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz b/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz new file mode 100644 index 00000000..2c455b9b --- /dev/null +++ b/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz @@ -0,0 +1,133 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 1037 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1328 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000015600000496fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000496000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000496fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000496000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f0000049600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 3074 + Y: 29 From a2257df3abc083b25f227db6247c289684ac391c Mon Sep 17 00:00:00 2001 From: Julian Date: Wed, 11 Jun 2025 18:37:41 +0200 Subject: [PATCH 38/60] running version --- graph_msf/CMakeLists.txt | 11 --------- .../smb_estimator_graph_ros2/CMakeLists.txt | 10 ++++++-- .../config/core/core_graph_params.yaml | 4 ++-- .../launch/smb_estimator_graph.launch.py | 3 +++ .../src/lib/SmbEstimator.cpp | 2 +- .../src/lib/SmbStaticTransforms.cpp | 24 +++++++++---------- .../extrinsics/StaticTransformsTf.h | 3 +-- .../graph_msf_ros2/ros/read_ros_params.h | 5 ++-- .../src/lib/StaticTransformsTf.cpp | 5 ++-- 9 files changed, 31 insertions(+), 36 deletions(-) diff --git a/graph_msf/CMakeLists.txt b/graph_msf/CMakeLists.txt index 384d6d67..88f197f9 100644 --- a/graph_msf/CMakeLists.txt +++ b/graph_msf/CMakeLists.txt @@ -24,14 +24,11 @@ message("CMAKE_BUILD_INCLUDE_DIR: ${CMAKE_BUILD_INCLUDE_DIR}") find_package(Eigen3 REQUIRED) find_package(GTSAM REQUIRED) find_package(GTSAM_UNSTABLE REQUIRED) -find_package(tf2_eigen REQUIRED) message("Eigen Version:: ${EIGEN3_VERSION_STRING}") message("Eigen Path:: ${EIGEN3_DIR}") message("GTSAM Include Path:: ${GTSAM_INCLUDE_DIRS}") -set(CMAKE_POSITION_INDEPENDENT_CODE ON) - # Color settings for terminal output if(NOT WIN32) string(ASCII 27 Esc) @@ -40,14 +37,6 @@ if(NOT WIN32) set(Magenta "${Esc}[35m") endif () -# Eigen -find_package(Eigen3 REQUIRED COMPONENTS) -# GTSAM -find_package(GTSAM REQUIRED) -find_package(GTSAM_UNSTABLE REQUIRED) -message("GTSAM Version:" ${GTSAM_VERSION}) -message("GTSAM Path:" ${GTSAM_DIR}) - ########### ## Build ## ########### diff --git a/ros2/examples/smb_estimator_graph_ros2/CMakeLists.txt b/ros2/examples/smb_estimator_graph_ros2/CMakeLists.txt index 0af82919..3c138700 100644 --- a/ros2/examples/smb_estimator_graph_ros2/CMakeLists.txt +++ b/ros2/examples/smb_estimator_graph_ros2/CMakeLists.txt @@ -18,8 +18,14 @@ find_package(nav_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(Glog REQUIRED) -find_package(graph_msf REQUIRED) -find_package(graph_msf_ros2 REQUIRED) + +# Check if the target from graph_msf is already defined +if(NOT TARGET graph_msf::graph_msf) + find_package(graph_msf REQUIRED) +endif() +if(NOT TARGET graph_msf_ros2::graph_msf_ros2) + find_package(graph_msf_ros2 REQUIRED) +endif() # Non propagated ros dependencies find_package(std_srvs REQUIRED) diff --git a/ros2/examples/smb_estimator_graph_ros2/config/core/core_graph_params.yaml b/ros2/examples/smb_estimator_graph_ros2/config/core/core_graph_params.yaml index fac5c153..e3ef892a 100644 --- a/ros2/examples/smb_estimator_graph_ros2/config/core/core_graph_params.yaml +++ b/ros2/examples/smb_estimator_graph_ros2/config/core/core_graph_params.yaml @@ -3,7 +3,7 @@ # Sensor Params sensor_params: imuRate: 200.0 # Rate of IMU input (Hz) - createStateEveryNthImuMeasurement: 4 # Create a new state every n-th IMU measurement + createStateEveryNthImuMeasurement: 5 # Create a new state every n-th IMU measurement useImuSignalLowPassFilter: false # If true, a low pass filter is applied to the IMU measurements imuLowPassFilterCutoffFreq: 30.0 # Cutoff frequency of the low pass filter imuBufferLength: 800 @@ -16,7 +16,7 @@ # Factor Graph graph_params: - realTimeSmootherLag: 10.0 # Lag of real-time fixed lag smoother[seconds] + realTimeSmootherLag: 2.0 # Lag of real-time fixed lag smoother[seconds] realTimeSmootherUseIsam: true realTimeSmootherUseCholeskyFactorization: true # CHOLESKY faster, QR numerically more stable useAdditionalSlowBatchSmoother: true # If true, a slower smoother is used in addition to the real-time smoother diff --git a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py index cd3c89b4..846c2abe 100644 --- a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py +++ b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py @@ -72,6 +72,9 @@ def generate_launch_description(): executable='smb_estimator_graph_ros2_node', name='smb_estimator_node', output='screen', + # prefix='gdb -ex run --args', + # prefix='xterm -e gdb -ex run --args', + # emulate_tty=True, parameters=[ {'use_sim_time': use_sim_time}, {'launch/optimizationResultLoggingPath': logging_dir_location}, diff --git a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp index 816b9917..fbb1f3ee 100644 --- a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp +++ b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp @@ -23,7 +23,7 @@ SmbEstimator::SmbEstimator(std::shared_ptr& node) : graph_msf::Gra REGULAR_COUT << GREEN_START << " SmbEstimator-Constructor called." << COLOR_END << std::endl; staticTransformsPtr_ = std::make_shared(node_); - + // Set the odometry flags in SmbStaticTransforms auto smbStaticTransforms = std::dynamic_pointer_cast(staticTransformsPtr_); if (smbStaticTransforms) { diff --git a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp index 5f2c6be5..a01c159d 100644 --- a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp +++ b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp @@ -19,9 +19,8 @@ Please see the LICENSE file that has been included as part of this package. namespace smb_se { -SmbStaticTransforms::SmbStaticTransforms(const std::shared_ptr& nodePtr) - : graph_msf::StaticTransformsTf(nodePtr, *this) { - REGULAR_COUT << GREEN_START << " Initializing static transforms..." << COLOR_END << std::endl; +SmbStaticTransforms::SmbStaticTransforms(const std::shared_ptr& nodePtr) : graph_msf::StaticTransformsTf(nodePtr) { + REGULAR_COUT << GREEN_START << " Initializing smb static transforms..." << COLOR_END << std::endl; } void SmbStaticTransforms::findTransformations() { @@ -56,16 +55,17 @@ void SmbStaticTransforms::findTransformations() { eigenTransform = tf2::transformToEigen(transform.transform); lv_T_frame1_frame2(imuFrame_, vioOdometryFrame_) = eigenTransform; - std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END << " Translation I_VIO: " << imuFrame_ << " " << vioOdometryFrame_ << " " - << rv_T_frame1_frame2(imuFrame_, vioOdometryFrame_).translation() << std::endl; + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END << " Translation I_VIO: " << imuFrame_ << " " << vioOdometryFrame_ + << " " << rv_T_frame1_frame2(imuFrame_, vioOdometryFrame_).translation() << std::endl; lv_T_frame1_frame2(vioOdometryFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, vioOdometryFrame_).inverse(); } // Wheel Frames --- if (useWheelOdometryBetweenFlag_ || useWheelLinearVelocitiesFlag_) { - REGULAR_COUT << RED_START - << " As the Wheels are turning, we only use the position of the wheels frames and use the orientation of the baseLinkFrame_ frame." - << COLOR_END << std::endl; + REGULAR_COUT + << RED_START + << " As the Wheels are turning, we only use the position of the wheels frames and use the orientation of the baseLinkFrame_ frame." + << COLOR_END << std::endl; // Left Wheel REGULAR_COUT << COLOR_END << " Waiting for transform for 10 seconds." << std::endl; @@ -74,8 +74,8 @@ void SmbStaticTransforms::findTransformations() { T_I_WheelLeft.matrix().block<3, 3>(0, 0) = rv_T_frame1_frame2(imuFrame_, baseLinkFrame_).matrix().block<3, 3>(0, 0); lv_T_frame1_frame2(imuFrame_, wheelLinearVelocityLeftFrame_) = T_I_WheelLeft; - std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END << " Translation I_WheelLeft: " - << rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityLeftFrame_).translation() + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END + << " Translation I_WheelLeft: " << rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityLeftFrame_).translation() << ", Rotation: " << T_I_WheelLeft.matrix().block<3, 3>(0, 0) << std::endl; lv_T_frame1_frame2(wheelLinearVelocityLeftFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityLeftFrame_).inverse(); @@ -86,8 +86,8 @@ void SmbStaticTransforms::findTransformations() { T_I_WheelRight.matrix().block<3, 3>(0, 0) = rv_T_frame1_frame2(imuFrame_, baseLinkFrame_).matrix().block<3, 3>(0, 0); lv_T_frame1_frame2(imuFrame_, wheelLinearVelocityRightFrame_) = T_I_WheelRight; - std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END << " Translation I_WheelRight: " - << rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityRightFrame_).translation() + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END + << " Translation I_WheelRight: " << rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityRightFrame_).translation() << ", Rotation: " << T_I_WheelRight.matrix().block<3, 3>(0, 0) << std::endl; lv_T_frame1_frame2(wheelLinearVelocityRightFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, wheelLinearVelocityRightFrame_).inverse(); } diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsTf.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsTf.h index a79e2374..dbf247b8 100644 --- a/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsTf.h +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsTf.h @@ -18,8 +18,7 @@ namespace graph_msf { class StaticTransformsTf : public StaticTransforms { public: - explicit StaticTransformsTf(const rclcpp::Node::SharedPtr& node, - const graph_msf::StaticTransforms& staticTransforms); + explicit StaticTransformsTf(const rclcpp::Node::SharedPtr& node); virtual ~StaticTransformsTf() = default; diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/ros/read_ros_params.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/ros/read_ros_params.h index 0d3af927..8b244b55 100644 --- a/ros2/graph_msf_ros2/include/graph_msf_ros2/ros/read_ros_params.h +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/ros/read_ros_params.h @@ -29,17 +29,16 @@ inline void printKey(const std::string& key, std::vector vector) { template T tryGetParam(std::shared_ptr node, const std::string& key) { T value; - RCLCPP_INFO(node->get_logger(), "Attempting to retrieve parameter: %s", key.c_str()); if (node->get_parameter(key, value)) { printKey(key, value); - RCLCPP_INFO(node->get_logger(), "Successfully retrieved parameter: %s", key.c_str()); + // RCLCPP_INFO(node->get_logger(), "Successfully retrieved parameter: %s", key.c_str()); return value; } if (node->get_parameter("/" + key, value)) { printKey("/" + key, value); - RCLCPP_INFO(node->get_logger(), "Successfully retrieved parameter with absolute key: %s", ("/" + key).c_str()); + // RCLCPP_INFO(node->get_logger(), "Successfully retrieved parameter with absolute key: %s", ("/" + key).c_str()); return value; } diff --git a/ros2/graph_msf_ros2/src/lib/StaticTransformsTf.cpp b/ros2/graph_msf_ros2/src/lib/StaticTransformsTf.cpp index be823232..bb5a9078 100644 --- a/ros2/graph_msf_ros2/src/lib/StaticTransformsTf.cpp +++ b/ros2/graph_msf_ros2/src/lib/StaticTransformsTf.cpp @@ -20,9 +20,8 @@ Please see the LICENSE file that has been included as part of this package. namespace graph_msf { -StaticTransformsTf::StaticTransformsTf(const rclcpp::Node::SharedPtr& node, - const graph_msf::StaticTransforms& staticTransforms) - : StaticTransforms(staticTransforms), +StaticTransformsTf::StaticTransformsTf(const rclcpp::Node::SharedPtr& node) + : StaticTransforms(), tf_buffer_(std::make_shared(node->get_clock())), tf_listener_(*tf_buffer_) { RCLCPP_INFO(rclcpp::get_logger("graph_msf"), "StaticTransformsTf - Initializing static transforms..."); From 0a48aa14c988af05a3b795aa74f93baaec405bd3 Mon Sep 17 00:00:00 2001 From: Julian Date: Wed, 11 Jun 2025 21:44:35 +0200 Subject: [PATCH 39/60] added use_sim_time and debugging --- .../launch/smb_estimator_graph.launch.py | 171 +++++++------ .../smb_estimator_graph_replay.launch.py | 61 ++++- .../rviz/rviz_config.rviz | 235 +++++++++++++++++- .../include/graph_msf_ros2/GraphMsfRos2.h | 3 + ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp | 10 +- 5 files changed, 375 insertions(+), 105 deletions(-) diff --git a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py index 846c2abe..050905aa 100644 --- a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py +++ b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py @@ -1,95 +1,106 @@ from launch import LaunchDescription +import launch from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory import os + def generate_launch_description(): # Package name - pkg_name = 'smb_estimator_graph_ros2' - + pkg_name = "smb_estimator_graph_ros2" + # Get package share directory pkg_dir = get_package_share_directory(pkg_name) - + # Declare launch arguments - use_sim_time = LaunchConfiguration('use_sim_time') - imu_topic_name = LaunchConfiguration('imu_topic_name') - lidar_odometry_topic_name = LaunchConfiguration('lidar_odometry_topic_name') - wheel_odometry_topic_name = LaunchConfiguration('wheel_odometry_topic_name') - wheel_velocities_topic_name = LaunchConfiguration('wheel_velocities_topic_name') - vio_odometry_topic_name = LaunchConfiguration('vio_odometry_topic_name') - logging_dir_location = LaunchConfiguration('logging_dir_location') + use_sim_time = LaunchConfiguration("use_sim_time", default="false") + imu_topic_name = LaunchConfiguration("imu_topic_name") + lidar_odometry_topic_name = LaunchConfiguration("lidar_odometry_topic_name") + wheel_odometry_topic_name = LaunchConfiguration("wheel_odometry_topic_name") + wheel_velocities_topic_name = LaunchConfiguration("wheel_velocities_topic_name") + vio_odometry_topic_name = LaunchConfiguration("vio_odometry_topic_name") + logging_dir_location = LaunchConfiguration("logging_dir_location") # Default config file paths - core_graph_config_param_file = os.path.join(pkg_dir, 'config', 'core', 'core_graph_config.yaml') - core_graph_param_file = os.path.join(pkg_dir, 'config', 'core', 'core_graph_params.yaml') - core_extrinsic_param_file = os.path.join(pkg_dir, 'config', 'core', 'core_extrinsic_params.yaml') - smb_graph_param_file = os.path.join(pkg_dir, 'config', 'smb_specific', 'smb_graph_params.yaml') - smb_extrinsic_param_file = os.path.join(pkg_dir, 'config', 'smb_specific', 'smb_extrinsic_params.yaml') - - return LaunchDescription([ - # Declare launch arguments - DeclareLaunchArgument( - 'use_sim_time', - default_value='false', - description='Use simulation time' - ), - DeclareLaunchArgument( - 'imu_topic_name', - default_value='/imu', - description='IMU topic name' - ), - DeclareLaunchArgument( - 'lidar_odometry_topic_name', - default_value='/mapping/scan2map_odometry', - description='Lidar odometry topic name' - ), - DeclareLaunchArgument( - 'wheel_odometry_topic_name', - default_value='/control/smb_diff_drive/odom', - description='Wheel odometry topic name' - ), - DeclareLaunchArgument( - 'wheel_velocities_topic_name', - default_value='/control/smb_lowlevel_controller/wheelSpeeds', - description='Wheel velocities topic name' - ), - DeclareLaunchArgument( - 'vio_odometry_topic_name', - default_value='/tracking_camera/odom/sample', - description='VIO odometry topic name' - ), - DeclareLaunchArgument( - 'logging_dir_location', - default_value=os.path.join(pkg_dir, 'logging'), - description='Logging directory location' - ), + core_graph_config_param_file = os.path.join( + pkg_dir, "config", "core", "core_graph_config.yaml" + ) + core_graph_param_file = os.path.join( + pkg_dir, "config", "core", "core_graph_params.yaml" + ) + core_extrinsic_param_file = os.path.join( + pkg_dir, "config", "core", "core_extrinsic_params.yaml" + ) + smb_graph_param_file = os.path.join( + pkg_dir, "config", "smb_specific", "smb_graph_params.yaml" + ) + smb_extrinsic_param_file = os.path.join( + pkg_dir, "config", "smb_specific", "smb_extrinsic_params.yaml" + ) - # Node - Node( - package='smb_estimator_graph_ros2', - executable='smb_estimator_graph_ros2_node', - name='smb_estimator_node', - output='screen', - # prefix='gdb -ex run --args', - # prefix='xterm -e gdb -ex run --args', - # emulate_tty=True, - parameters=[ - {'use_sim_time': use_sim_time}, - {'launch/optimizationResultLoggingPath': logging_dir_location}, - core_graph_config_param_file, - core_graph_param_file, - core_extrinsic_param_file, - smb_graph_param_file, - smb_extrinsic_param_file - ], - remappings=[ - ('/imu_topic', imu_topic_name), - ('/lidar_odometry_topic', lidar_odometry_topic_name), - ('/wheel_odometry_topic', wheel_odometry_topic_name), - ('/wheel_velocities_topic', wheel_velocities_topic_name), - ('/vio_odometry_topic', vio_odometry_topic_name) - ] - ) - ]) \ No newline at end of file + return LaunchDescription( + [ + # Declare launch arguments + DeclareLaunchArgument( + "use_sim_time", default_value=use_sim_time, description="Use simulation time" + ), + DeclareLaunchArgument( + "imu_topic_name", default_value="/imu", description="IMU topic name" + ), + DeclareLaunchArgument( + "lidar_odometry_topic_name", + default_value="/mapping/scan2map_odometry", + description="Lidar odometry topic name", + ), + DeclareLaunchArgument( + "wheel_odometry_topic_name", + default_value="/control/smb_diff_drive/odom", + description="Wheel odometry topic name", + ), + DeclareLaunchArgument( + "wheel_velocities_topic_name", + default_value="/control/smb_lowlevel_controller/wheelSpeeds", + description="Wheel velocities topic name", + ), + DeclareLaunchArgument( + "vio_odometry_topic_name", + default_value="/tracking_camera/odom/sample", + description="VIO odometry topic name", + ), + DeclareLaunchArgument( + "logging_dir_location", + default_value=os.path.join(pkg_dir, "logging"), + description="Logging directory location", + ), + # Set the /use_sim_time parameter for all nodes + launch.actions.SetLaunchConfiguration("use_sim_time", launch.substitutions.LaunchConfiguration("use_sim_time")), + # Main Node + Node( + package="smb_estimator_graph_ros2", + executable="smb_estimator_graph_ros2_node", + name="smb_estimator_node", + output="screen", + # prefix='gdb -ex run --args', + # prefix='xterm -e gdb -ex run --args', + # emulate_tty=True, + parameters=[ + {"use_sim_time": use_sim_time}, + {"launch/optimizationResultLoggingPath": logging_dir_location}, + core_graph_config_param_file, + core_graph_param_file, + core_extrinsic_param_file, + smb_graph_param_file, + smb_extrinsic_param_file, + ], + remappings=[ + ("/imu_topic", imu_topic_name), + ("/lidar_odometry_topic", lidar_odometry_topic_name), + ("/wheel_odometry_topic", wheel_odometry_topic_name), + ("/wheel_velocities_topic", wheel_velocities_topic_name), + ("/vio_odometry_topic", vio_odometry_topic_name), + ], + ), + ] + ) diff --git a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py index 7ba0f39f..088dd8b7 100644 --- a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py +++ b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py @@ -1,4 +1,5 @@ from launch import LaunchDescription +import launch from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -14,48 +15,84 @@ def generate_launch_description(): # Get package share directory pkg_dir = get_package_share_directory(pkg_name) + # Sim Time + use_sim_time_str = launch.substitutions.LaunchConfiguration( + "use_sim_time", default="true" + ).perform(launch.LaunchContext()) + use_sim_time = use_sim_time_str.lower() == "true" # Convert to Python bool + print(f"Use Sim Time: {use_sim_time}") + # Declare launch arguments - use_sim_time = LaunchConfiguration("use_sim_time") imu_topic_name = LaunchConfiguration("imu_topic_name") lidar_odometry_topic_name = LaunchConfiguration("lidar_odometry_topic_name") core_graph_param_file = LaunchConfiguration("core_graph_param_file") smb_graph_param_file = LaunchConfiguration("smb_graph_param_file") # Default config file paths - default_core_graph_param_file = os.path.join(pkg_dir, "config", "core", "core_graph_params.yaml") - default_smb_graph_param_file = os.path.join(pkg_dir, "config", "smb_specific", "smb_graph_params.yaml") + default_core_graph_param_file = os.path.join( + pkg_dir, "config", "core", "core_graph_params.yaml" + ) + default_smb_graph_param_file = os.path.join( + pkg_dir, "config", "smb_specific", "smb_graph_params.yaml" + ) return LaunchDescription( [ # Declare launch arguments - DeclareLaunchArgument("use_sim_time", default_value="true", description="Use simulation time"), - DeclareLaunchArgument("imu_topic_name", default_value="/imu", description="IMU topic name"), + # A. Sim time + DeclareLaunchArgument( + "use_sim_time", + default_value=use_sim_time_str.lower(), + description="Use simulated time", + ), + # B. Topics + DeclareLaunchArgument( + "imu_topic_name", default_value="/imu", description="IMU topic name" + ), DeclareLaunchArgument( - "lidar_odometry_topic_name", default_value="/mapping/scan2map_odometry", description="Lidar odometry topic name" + "lidar_odometry_topic_name", + default_value="/mapping/scan2map_odometry", + description="Lidar odometry topic name", ), + # C. Parameter files DeclareLaunchArgument( - "core_graph_param_file", default_value=default_core_graph_param_file, description="Core graph parameter file" + "core_graph_param_file", + default_value=default_core_graph_param_file, + description="Core graph parameter file", ), DeclareLaunchArgument( - "smb_graph_param_file", default_value=default_smb_graph_param_file, description="SMB graph parameter file" + "smb_graph_param_file", + default_value=default_smb_graph_param_file, + description="SMB graph parameter file", + ), + # D. Set the /use_sim_time parameter for all nodes + launch.actions.SetLaunchConfiguration( + "use_sim_time", launch.substitutions.LaunchConfiguration("use_sim_time") ), - # Include the main launch file + # E. Include the main launch file IncludeLaunchDescription( - PythonLaunchDescriptionSource([os.path.join(pkg_dir, "launch", "smb_estimator_graph.launch.py")]), + PythonLaunchDescriptionSource( + [os.path.join(pkg_dir, "launch", "smb_estimator_graph.launch.py")] + ), launch_arguments={ - "use_sim_time": use_sim_time, + "use_sim_time": launch.substitutions.LaunchConfiguration( + "use_sim_time" + ), "imu_topic_name": imu_topic_name, "lidar_odometry_topic_name": lidar_odometry_topic_name, "core_graph_param_file": core_graph_param_file, "smb_graph_param_file": smb_graph_param_file, }.items(), ), - # RViz Node + # F. RVIZ Node Node( package="rviz2", executable="rviz2", name="rviz2", arguments=["-d", os.path.join(pkg_dir, "rviz", "rviz_config.rviz")], + parameters=[ + {"use_sim_time": use_sim_time}, + ], output="screen", ), ] diff --git a/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz b/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz index 2c455b9b..b813b522 100644 --- a/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz +++ b/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz @@ -7,7 +7,7 @@ Panels: - /Global Options1 - /Status1 Splitter Ratio: 0.5 - Tree Height: 1037 + Tree Height: 1031 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -47,10 +47,221 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: true + LF_WHEEL: + Value: true + LH_WHEEL: + Value: true + RF_WHEEL: + Value: true + RH_WHEEL: + Value: true + base_footprint: + Value: true + base_inertia: + Value: true + base_link: + Value: true + imu_link: + Value: true + imu_sensor: + Value: true + lidar_mount_link: + Value: true + map: + Value: true + odom: + Value: true + odom_graph_msf: + Value: true + rgb_camera_link: + Value: true + rgb_camera_optical_link: + Value: true + rslidar: + Value: true + top: + Value: true + tracking_camera_link: + Value: true + tracking_camera_pose_frame: + Value: true + world_graph_msf: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + odom: + base_link: + LF_WHEEL: + {} + LH_WHEEL: + {} + RF_WHEEL: + {} + RH_WHEEL: + {} + base_footprint: + {} + base_inertia: + {} + imu_link: + imu_sensor: + {} + lidar_mount_link: + {} + odom_graph_msf: + world_graph_msf: + {} + rgb_camera_link: + rgb_camera_optical_link: + {} + rslidar: + {} + top: + {} + tracking_camera_link: + tracking_camera_pose_frame: + {} + Update Interval: 0 + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 100 + Name: OdometryInOdom + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /graph_msf/est_odometry_odom_imu + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 255; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: PathInOdom + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /graph_msf/est_path_odom_imu + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: MeasPathLio + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /graph_msf/measLiDAR_path_map_imu + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: OptimizedPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /graph_msf/opt_path_world_imu + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: map + Fixed Frame: odom_graph_msf Frame Rate: 30 Name: root Tools: @@ -93,33 +304,33 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 10 + Distance: 9.480110168457031 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0 - Y: 0 - Z: 0 + X: -0.9547891616821289 + Y: -2.3312158584594727 + Z: -0.17786170542240143 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.785398006439209 + Pitch: 0.38039833307266235 Target Frame: Value: Orbit (rviz) - Yaw: 0.785398006439209 + Yaw: 0.9203998446464539 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1328 Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000496fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000496000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000496fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000496000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f0000049600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000015600000492fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000492000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000492fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000492000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000026600fffffffb0000000800540069006d00650100000000000004500000000000000000000003540000049200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -127,7 +338,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: false + collapsed: true Width: 1200 - X: 3074 + X: 3659 Y: 29 diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h index 8327d603..fdbc878a 100644 --- a/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h @@ -110,6 +110,9 @@ class GraphMsfRos2 : public GraphMsfClassic, public GraphMsfHolistic { // Node std::shared_ptr node_; + // Clock + rclcpp::Clock::SharedPtr clock_; + // Time std::chrono::time_point startTime; std::chrono::time_point currentTime; diff --git a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp index ac803638..89fa291e 100644 --- a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp +++ b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp @@ -26,6 +26,9 @@ void GraphMsfRos2::setup(std::shared_ptr staticTransformsPtr) throw std::runtime_error("Static transforms not set. Has to be set."); } + // Clock + clock_ = node_->get_clock(); + // Sensor Params node_->declare_parameter("sensor_params.imuRate", 0.0); node_->declare_parameter("sensor_params.createStateEveryNthImuMeasurement", 25); @@ -404,10 +407,15 @@ void GraphMsfRos2::imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuMsgPtr) linearAcc, angularVel, imuMsgPtr->header.stamp.sec + 1e-9 * imuMsgPtr->header.stamp.nanosec, preIntegratedNavStatePtr, optimizedStateWithCovarianceAndBiasPtr, addedImuMeasurements)) { // Encountered Delay - auto now = node_->now(); + auto now = clock_->now(); auto delay = now.seconds() - preIntegratedNavStatePtr->getTimeK(); if (delay > 0.5) { RCLCPP_WARN(node_->get_logger(), "Encountered delay of %.14f seconds.", delay); + // Print now vs chrono time + auto currentChronoTime = std::chrono::high_resolution_clock::now(); + auto chronoTimeSinceEpoch = std::chrono::duration_cast>(currentChronoTime.time_since_epoch()).count(); + RCLCPP_WARN(node_->get_logger(), "Now: %.14f, Std chrono time: %.14f", + now.seconds(), chronoTimeSinceEpoch); } // Publish Odometry From 60616e0f08891a40e754ac948526250c85bd4c81 Mon Sep 17 00:00:00 2001 From: Julian Date: Mon, 16 Jun 2025 00:35:54 +0200 Subject: [PATCH 40/60] more efficeint and path vis --- .../rviz/rviz_config.rviz | 23 +++++++++++-------- .../src/lib/SmbEstimator.cpp | 18 +++++++++++++-- 2 files changed, 29 insertions(+), 12 deletions(-) diff --git a/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz b/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz index b813b522..0a2864f4 100644 --- a/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz +++ b/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz @@ -80,6 +80,8 @@ Visualization Manager: Value: true odom_graph_msf: Value: true + odom_graph_msf_aligned: + Value: true rgb_camera_link: Value: true rgb_camera_optical_link: @@ -122,7 +124,8 @@ Visualization Manager: {} odom_graph_msf: world_graph_msf: - {} + odom_graph_msf_aligned: + {} rgb_camera_link: rgb_camera_optical_link: {} @@ -135,7 +138,7 @@ Visualization Manager: {} Update Interval: 0 Value: true - - Angle Tolerance: 0.10000000149011612 + - Angle Tolerance: 0 Class: rviz_default_plugins/Odometry Covariance: Orientation: @@ -153,9 +156,9 @@ Visualization Manager: Value: true Value: true Enabled: true - Keep: 100 + Keep: 1 Name: OdometryInOdom - Position Tolerance: 0.10000000149011612 + Position Tolerance: 0 Shape: Alpha: 1 Axes Length: 1 @@ -304,22 +307,22 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 9.480110168457031 + Distance: 14.281952857971191 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.9547891616821289 - Y: -2.3312158584594727 - Z: -0.17786170542240143 + X: -2.2798728942871094 + Y: -1.0027551651000977 + Z: -0.8153277635574341 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.38039833307266235 + Pitch: 0.5053982138633728 Target Frame: Value: Orbit (rviz) Yaw: 0.9203998446464539 @@ -340,5 +343,5 @@ Window Geometry: Views: collapsed: true Width: 1200 - X: 3659 + X: 3633 Y: 29 diff --git a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp index fbb1f3ee..847dfb45 100644 --- a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp +++ b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp @@ -149,11 +149,25 @@ void SmbEstimator::imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuPtr) { void SmbEstimator::lidarOdometryCallback_(const nav_msgs::msg::Odometry::ConstSharedPtr& odomLidarPtr) { static int lidarOdometryCallbackCounter__ = -1; + static double lastLidarOdometryTimeK_ = 0.0; + static constexpr double lioOdometryRate_ = 10.0; // Hz + + // Timestamp + double lidarOdometryTimeK = odomLidarPtr->header.stamp.sec + odomLidarPtr->header.stamp.nanosec * 1e-9; + + // Check whether the callback rate is not exceeded + if (lidarOdometryCallbackCounter__ >= 0 && lidarOdometryTimeK - lastLidarOdometryTimeK_ < (1.0 / lioOdometryRate_)) { + return; // Skip this callback if the rate is exceeded + } else { + lastLidarOdometryTimeK_ = lidarOdometryTimeK; // Update the last timestamp + } + + // Update the callback counter ++lidarOdometryCallbackCounter__; Eigen::Isometry3d lio_T_M_Lk; graph_msf::odomMsgToEigen(*odomLidarPtr, lio_T_M_Lk.matrix()); - double lidarOdometryTimeK = odomLidarPtr->header.stamp.sec + odomLidarPtr->header.stamp.nanosec * 1e-9; + const std::string& lioOdometryFrame = dynamic_cast(staticTransformsPtr_.get())->getLioOdometryFrame(); @@ -170,7 +184,7 @@ void SmbEstimator::lidarOdometryCallback_(const nav_msgs::msg::Odometry::ConstSh this->initYawAndPosition(unary6DMeasurement); } - addToPathMsg(measLio_mapImuPathPtr_, odomLidarPtr->header.frame_id, odomLidarPtr->header.stamp, + addToPathMsg(measLio_mapImuPathPtr_, odomLidarPtr->header.frame_id + referenceFrameAlignedNameId, odomLidarPtr->header.stamp, (lio_T_M_Lk * staticTransformsPtr_->rv_T_frame1_frame2(lioOdometryFrame, staticTransformsPtr_->getImuFrame()).matrix()) .block<3, 1>(0, 3), graphConfigPtr_->imuBufferLength_ * 4); From d162e982a1c02c3f3870655d82d546bdc0ff7929 Mon Sep 17 00:00:00 2001 From: Julian Date: Mon, 16 Jun 2025 21:58:01 +0200 Subject: [PATCH 41/60] fixing of bug and better visualization --- .../config/core/core_extrinsic_params.yaml | 2 +- .../smb_specific/smb_extrinsic_params.yaml | 2 +- .../rviz/rviz_config.rviz | 113 +++++++++++------- ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp | 8 +- 4 files changed, 78 insertions(+), 47 deletions(-) diff --git a/ros2/examples/smb_estimator_graph_ros2/config/core/core_extrinsic_params.yaml b/ros2/examples/smb_estimator_graph_ros2/config/core/core_extrinsic_params.yaml index b42c84dd..8a042bf7 100644 --- a/ros2/examples/smb_estimator_graph_ros2/config/core/core_extrinsic_params.yaml +++ b/ros2/examples/smb_estimator_graph_ros2/config/core/core_extrinsic_params.yaml @@ -8,7 +8,7 @@ # Used for estimation imuFrame: "imu_link" baseLinkFrame: "base_link" - initializeZeroYawAndPositionOfFrame: "rslidar" # Initialize the yaw and position of the base frame to zero + initializeZeroYawAndPositionOfFrame: "base_link" # Initialize the yaw and position of the base frame to zero # this is just meant to avoid any jump after getting the first true measurements name_ids: diff --git a/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_extrinsic_params.yaml b/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_extrinsic_params.yaml index 765ec560..7bd16869 100644 --- a/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_extrinsic_params.yaml +++ b/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_extrinsic_params.yaml @@ -2,7 +2,7 @@ ros__parameters: #External Prior/Transform Input extrinsics: - lidarOdometryFrame: "rslidar" #LiDAR frame name - used to lookup LiDAR-to-ExternalSensor Frame + lidarOdometryFrame: "base_link" #LiDAR frame name - used to lookup LiDAR-to-ExternalSensor Frame wheelOdometryBetweenFrame: "base_link" #Wheel odometry frame name - used to lookup Wheel-to-ExternalSensor Frame wheelLinearVelocityLeftFrame: "LH_WHEEL" #Wheel linear velocity left frame name - used to lookup Wheel-to-ExternalSensor Frame wheelLinearVelocityRightFrame: "RH_WHEEL" #Wheel linear velocity right frame name - used to lookup Wheel-to-ExternalSensor Frame diff --git a/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz b/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz index 0a2864f4..ee9766de 100644 --- a/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz +++ b/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz @@ -6,7 +6,9 @@ Panels: Expanded: - /Global Options1 - /Status1 - Splitter Ratio: 0.5 + - /TF1 + - /TF1/Frames1 + Splitter Ratio: 0.6352941393852234 Tree Height: 1031 - Class: rviz_common/Selection Name: Selection @@ -102,40 +104,41 @@ Visualization Manager: Show Axes: true Show Names: false Tree: + base_link: + LF_WHEEL: + {} + LH_WHEEL: + {} + RF_WHEEL: + {} + RH_WHEEL: + {} + base_footprint: + {} + base_inertia: + {} + imu_link: + imu_sensor: + {} + lidar_mount_link: + {} + odom_graph_msf: + world_graph_msf: + odom_graph_msf_aligned: + {} + rgb_camera_link: + rgb_camera_optical_link: + {} + rslidar: + {} + top: + {} + tracking_camera_link: + tracking_camera_pose_frame: + {} map: odom: - base_link: - LF_WHEEL: - {} - LH_WHEEL: - {} - RF_WHEEL: - {} - RH_WHEEL: - {} - base_footprint: - {} - base_inertia: - {} - imu_link: - imu_sensor: - {} - lidar_mount_link: - {} - odom_graph_msf: - world_graph_msf: - odom_graph_msf_aligned: - {} - rgb_camera_link: - rgb_camera_optical_link: - {} - rslidar: - {} - top: - {} - tracking_camera_link: - tracking_camera_pose_frame: - {} + {} Update Interval: 0 Value: true - Angle Tolerance: 0 @@ -205,6 +208,34 @@ Visualization Manager: Reliability Policy: Reliable Value: /graph_msf/est_path_odom_imu Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 85; 170; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: PathInWorld + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /graph_msf/est_path_world_imu + Value: true - Alpha: 1 Buffer Length: 1 Class: rviz_default_plugins/Path @@ -307,25 +338,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 14.281952857971191 + Distance: 10 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -2.2798728942871094 - Y: -1.0027551651000977 - Z: -0.8153277635574341 - Focal Shape Fixed Size: true + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5053982138633728 + Pitch: 0.7853981852531433 Target Frame: Value: Orbit (rviz) - Yaw: 0.9203998446464539 + Yaw: 0.7853981852531433 Saved: ~ Window Geometry: Displays: @@ -343,5 +374,5 @@ Window Geometry: Views: collapsed: true Width: 1200 - X: 3633 + X: 3914 Y: 29 diff --git a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp index 89fa291e..ece2ad56 100644 --- a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp +++ b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp @@ -549,15 +549,15 @@ void GraphMsfRos2::publishOptimizedStateAndBias( std::string mapFrameName; const std::string& worldFrameName = staticTransformsPtr_->getWorldFrame(); - // If world is first, then map is second - if (framePairTransformMapIterator.first.first == worldFrameName) { + // If world is second, then map is first + if (framePairTransformMapIterator.first.second == worldFrameName) { T_W_M = framePairTransformMapIterator.second.inverse(); - mapFrameName = framePairTransformMapIterator.first.second; + mapFrameName = framePairTransformMapIterator.first.first; } // If world is second, then map is first, this is the case for holistic alignment else { T_W_M = framePairTransformMapIterator.second; - mapFrameName = framePairTransformMapIterator.first.first; + mapFrameName = framePairTransformMapIterator.first.second; // B. Publish TransformStamped for Aligned Frames std::string transformTopic = From de562e473e3a22dfac3b08e266f67466fa4802bc Mon Sep 17 00:00:00 2001 From: Julian Date: Mon, 16 Jun 2025 23:42:30 +0200 Subject: [PATCH 42/60] bit more tuning --- .../config/smb_specific/smb_graph_params.yaml | 4 +- .../smb_estimator_graph_replay.launch.py | 2 +- .../rviz/rviz_config.rviz | 221 ++++++++++++++---- .../src/lib/SmbEstimator.cpp | 2 +- 4 files changed, 176 insertions(+), 53 deletions(-) diff --git a/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml b/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml index 8452aee8..def6519a 100644 --- a/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml +++ b/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml @@ -17,13 +17,13 @@ # Alignment Parameters alignment_params: - initialSe3AlignmentNoiseDensity: [ 1.0e-01, 1.0e-01, 1.0e-01, 1.0e-02, 1.0e-02, 1.0e-02 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + initialSe3AlignmentNoiseDensity: [ 1.0e-02, 1.0e-02, 1.0e-02, 1.0e-02, 1.0e-02, 1.0e-02 ] # StdDev, ORDER RPY(rad) - XYZ(meters) lioSe3AlignmentRandomWalk: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] # StdDev, ORDER RPY(rad) - XYZ(meters) # Noise Parameters noise_params: ## LiDAR - lioPoseUnaryNoiseDensity: [ 0.05, 0.05, 0.05, 0.01, 0.01, 0.01 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + lioPoseUnaryNoiseDensity: [ 0.05, 0.05, 0.05, 0.05, 0.05, 0.05 ] # StdDev, ORDER RPY(rad) - XYZ(meters) ## Wheel wheelPoseBetweenNoiseDensity: [ 0.1, 0.1, 0.1, 0.06, 0.06, 0.06 ] # StdDev, ORDER RPY(rad) - XYZ(meters) wheelLinearVelocitiesNoiseDensity: [ 0.3, 0.3, 0.3 ] # StdDev, ORDER VxVyVz(meters/sec) diff --git a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py index 088dd8b7..fe716fd9 100644 --- a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py +++ b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py @@ -47,7 +47,7 @@ def generate_launch_description(): ), # B. Topics DeclareLaunchArgument( - "imu_topic_name", default_value="/imu", description="IMU topic name" + "imu_topic_name", default_value="/imu/data_raw", description="IMU topic name" ), DeclareLaunchArgument( "lidar_odometry_topic_name", diff --git a/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz b/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz index ee9766de..9cc7337b 100644 --- a/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz +++ b/ros2/examples/smb_estimator_graph_ros2/rviz/rviz_config.rviz @@ -6,7 +6,6 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /TF1 - /TF1/Frames1 Splitter Ratio: 0.6352941393852234 Tree Height: 1031 @@ -27,7 +26,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: PointCloud2 Visualization Manager: Class: "" Displays: @@ -56,14 +55,6 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: true - LF_WHEEL: - Value: true - LH_WHEEL: - Value: true - RF_WHEEL: - Value: true - RH_WHEEL: - Value: true base_footprint: Value: true base_inertia: @@ -76,8 +67,6 @@ Visualization Manager: Value: true lidar_mount_link: Value: true - map: - Value: true odom: Value: true odom_graph_msf: @@ -104,41 +93,31 @@ Visualization Manager: Show Axes: true Show Names: false Tree: - base_link: - LF_WHEEL: - {} - LH_WHEEL: - {} - RF_WHEEL: - {} - RH_WHEEL: - {} - base_footprint: - {} - base_inertia: - {} - imu_link: - imu_sensor: + odom: + base_link: + base_footprint: + {} + base_inertia: + {} + imu_link: + imu_sensor: + {} + lidar_mount_link: {} - lidar_mount_link: - {} - odom_graph_msf: - world_graph_msf: - odom_graph_msf_aligned: + odom_graph_msf: + world_graph_msf: + odom_graph_msf_aligned: + {} + rgb_camera_link: + rgb_camera_optical_link: {} - rgb_camera_link: - rgb_camera_optical_link: + rslidar: {} - rslidar: - {} - top: - {} - tracking_camera_link: - tracking_camera_pose_frame: + top: {} - map: - odom: - {} + tracking_camera_link: + tracking_camera_pose_frame: + {} Update Interval: 0 Value: true - Angle Tolerance: 0 @@ -158,7 +137,7 @@ Visualization Manager: Scale: 1 Value: true Value: true - Enabled: true + Enabled: false Keep: 1 Name: OdometryInOdom Position Tolerance: 0 @@ -179,7 +158,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /graph_msf/est_odometry_odom_imu - Value: true + Value: false - Alpha: 1 Buffer Length: 1 Class: rviz_default_plugins/Path @@ -292,6 +271,150 @@ Visualization Manager: Reliability Policy: Reliable Value: /graph_msf/opt_path_world_imu Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + LF_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LH_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + RF_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_sensor: + Alpha: 1 + Show Axes: false + Show Trail: false + lidar_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + rgb_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + rgb_camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + rslidar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top: + Alpha: 1 + Show Axes: false + Show Trail: false + tracking_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tracking_camera_pose_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + "": true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /graph_msf/velocity_marker + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 204 + Min Color: 0; 0; 0 + Min Intensity: 14 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rslidar/points + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -338,7 +461,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 10 + Distance: 22.106815338134766 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -364,7 +487,7 @@ Window Geometry: Height: 1328 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000015600000492fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000492000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000492fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000492000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000026600fffffffb0000000800540069006d00650100000000000004500000000000000000000003540000049200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015600000492fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000492000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000492fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000492000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007320000003efc0100000002fb0000000800540069006d00650100000000000007320000026600fffffffb0000000800540069006d00650100000000000004500000000000000000000005d60000049200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -373,6 +496,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1200 - X: 3914 + Width: 1842 + X: 3272 Y: 29 diff --git a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp index 847dfb45..d2ca97b0 100644 --- a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp +++ b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp @@ -173,7 +173,7 @@ void SmbEstimator::lidarOdometryCallback_(const nav_msgs::msg::Odometry::ConstSh graph_msf::UnaryMeasurementXDAbsolute unary6DMeasurement( "Lidar_unary_6D", int(lioOdometryRate_), lioOdometryFrame, lioOdometryFrame + sensorFrameCorrectedNameId, - graph_msf::RobustNorm::None(), lidarOdometryTimeK, 1.0, lio_T_M_Lk, lioPoseUnaryNoise_, odomLidarPtr->header.frame_id, + graph_msf::RobustNorm::Huber(1), lidarOdometryTimeK, 1.0, lio_T_M_Lk, lioPoseUnaryNoise_, odomLidarPtr->header.frame_id, staticTransformsPtr_->getWorldFrame(), initialSe3AlignmentNoise_, lioSe3AlignmentRandomWalk_); if (lidarOdometryCallbackCounter__ <= 2) { From 3bf4c80befe6fa18a06fd52a4b021ebdae43a148 Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Thu, 19 Jun 2025 12:01:55 +0000 Subject: [PATCH 43/60] Update IMU and Lidar odometry topic names in launch file for improved simulation configuration --- .../launch/smb_estimator_graph_sim.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_sim.launch.py b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_sim.launch.py index 5ebef284..c5f5ef22 100644 --- a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_sim.launch.py +++ b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_sim.launch.py @@ -37,12 +37,12 @@ def generate_launch_description(): ), DeclareLaunchArgument( 'imu_topic_name', - default_value='/imu', + default_value='/imu/data_raw', description='IMU topic name' ), DeclareLaunchArgument( 'lidar_odometry_topic_name', - default_value='/dlio/odom_node/odom', + default_value='/open3d/scan2map_odometry', description='Lidar odometry topic name' ), DeclareLaunchArgument( From e3c5c42f2471b3a605acfed92686e22a21aa8403 Mon Sep 17 00:00:00 2001 From: tutuna Date: Thu, 19 Jun 2025 16:02:44 +0200 Subject: [PATCH 44/60] Make graph msf compatible with Open3D SLAM instead of DLIO --- .../config/smb_specific/smb_extrinsic_params.yaml | 2 +- .../config/smb_specific/smb_graph_params.yaml | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_extrinsic_params.yaml b/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_extrinsic_params.yaml index 7bd16869..765ec560 100644 --- a/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_extrinsic_params.yaml +++ b/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_extrinsic_params.yaml @@ -2,7 +2,7 @@ ros__parameters: #External Prior/Transform Input extrinsics: - lidarOdometryFrame: "base_link" #LiDAR frame name - used to lookup LiDAR-to-ExternalSensor Frame + lidarOdometryFrame: "rslidar" #LiDAR frame name - used to lookup LiDAR-to-ExternalSensor Frame wheelOdometryBetweenFrame: "base_link" #Wheel odometry frame name - used to lookup Wheel-to-ExternalSensor Frame wheelLinearVelocityLeftFrame: "LH_WHEEL" #Wheel linear velocity left frame name - used to lookup Wheel-to-ExternalSensor Frame wheelLinearVelocityRightFrame: "RH_WHEEL" #Wheel linear velocity right frame name - used to lookup Wheel-to-ExternalSensor Frame diff --git a/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml b/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml index def6519a..7216a8c0 100644 --- a/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml +++ b/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml @@ -8,7 +8,7 @@ useWheelLinearVelocities: false useVioOdometry: false ## Rates - lioOdometryRate: 100 + lioOdometryRate: 10 wheelOdometryBetweenRate: 50 wheelLinearVelocitiesRate: 50 vioOdometryRate: 50 @@ -23,7 +23,7 @@ # Noise Parameters noise_params: ## LiDAR - lioPoseUnaryNoiseDensity: [ 0.05, 0.05, 0.05, 0.05, 0.05, 0.05 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + lioPoseUnaryNoiseDensity: [ 0.02, 0.02, 0.02, 0.02, 0.02, 0.02 ] # StdDev, ORDER RPY(rad) - XYZ(meters) ## Wheel wheelPoseBetweenNoiseDensity: [ 0.1, 0.1, 0.1, 0.06, 0.06, 0.06 ] # StdDev, ORDER RPY(rad) - XYZ(meters) wheelLinearVelocitiesNoiseDensity: [ 0.3, 0.3, 0.3 ] # StdDev, ORDER VxVyVz(meters/sec) From d1ab1599bbbd2b8de027551803983129a9992e04 Mon Sep 17 00:00:00 2001 From: Julian Date: Thu, 19 Jun 2025 17:21:34 +0200 Subject: [PATCH 45/60] latest updates --- .../graph_msf/config/StaticTransforms.h | 2 +- .../SmbStaticTransforms.h | 2 +- .../launch/smb_estimator_graph.launch.py | 2 +- .../src/lib/SmbEstimator.cpp | 11 +- .../src/lib/SmbStaticTransforms.cpp | 14 +- .../include/graph_msf_ros2/GraphMsfRos2bu.h | 176 ------------------ .../extrinsics/StaticTransformsTf.h | 2 +- .../graph_msf_ros2/src/lib/GraphMsfRos2bu.cpp | 167 ----------------- .../src/lib/StaticTransformsTf.cpp | 24 ++- 9 files changed, 32 insertions(+), 368 deletions(-) delete mode 100644 ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2bu.h delete mode 100644 ros2/graph_msf_ros2/src/lib/GraphMsfRos2bu.cpp diff --git a/graph_msf/include/graph_msf/config/StaticTransforms.h b/graph_msf/include/graph_msf/config/StaticTransforms.h index 45697469..3d50d14f 100644 --- a/graph_msf/include/graph_msf/config/StaticTransforms.h +++ b/graph_msf/include/graph_msf/config/StaticTransforms.h @@ -38,7 +38,7 @@ class StaticTransforms : public TransformsDictionary { const std::string& getInitializationFrame() { return initializationFrame_; } // Functionality ------------------------------------------------------------ - virtual void findTransformations() = 0; + virtual bool findTransformations() = 0; protected: // Required frames diff --git a/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h b/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h index eed44167..78f990e4 100644 --- a/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h +++ b/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbStaticTransforms.h @@ -39,7 +39,7 @@ class SmbStaticTransforms : public graph_msf::StaticTransformsTf { void setUseWheelLinearVelocitiesFlag(bool flag) { useWheelLinearVelocitiesFlag_ = flag; } private: - void findTransformations() override; + bool findTransformations() override; // Frames std::string lidarOdometryFrame_; diff --git a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py index 050905aa..c48c0606 100644 --- a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py +++ b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py @@ -47,7 +47,7 @@ def generate_launch_description(): "use_sim_time", default_value=use_sim_time, description="Use simulation time" ), DeclareLaunchArgument( - "imu_topic_name", default_value="/imu", description="IMU topic name" + "imu_topic_name", default_value="/imu/data_raw", description="IMU topic name" ), DeclareLaunchArgument( "lidar_odometry_topic_name", diff --git a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp index d2ca97b0..27deb2f3 100644 --- a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp +++ b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp @@ -82,8 +82,13 @@ void SmbEstimator::setup() { GraphMsfRos2::setup(staticTransformsPtr_); - // Transforms - staticTransformsPtr_->findTransformations(); + // Transforms --> query until returns true + bool foundTransforms = false; + while (!foundTransforms) { + foundTransforms = staticTransformsPtr_->findTransformations(); + // Sleep for 0.1 seconds to avoid busy waiting + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } REGULAR_COUT << GREEN_START << " Set up successfully." << COLOR_END << std::endl; } @@ -173,7 +178,7 @@ void SmbEstimator::lidarOdometryCallback_(const nav_msgs::msg::Odometry::ConstSh graph_msf::UnaryMeasurementXDAbsolute unary6DMeasurement( "Lidar_unary_6D", int(lioOdometryRate_), lioOdometryFrame, lioOdometryFrame + sensorFrameCorrectedNameId, - graph_msf::RobustNorm::Huber(1), lidarOdometryTimeK, 1.0, lio_T_M_Lk, lioPoseUnaryNoise_, odomLidarPtr->header.frame_id, + graph_msf::RobustNorm::None(), lidarOdometryTimeK, 1.0, lio_T_M_Lk, lioPoseUnaryNoise_, odomLidarPtr->header.frame_id, staticTransformsPtr_->getWorldFrame(), initialSe3AlignmentNoise_, lioSe3AlignmentRandomWalk_); if (lidarOdometryCallbackCounter__ <= 2) { diff --git a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp index a01c159d..9d21829b 100644 --- a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp +++ b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbStaticTransforms.cpp @@ -23,13 +23,16 @@ SmbStaticTransforms::SmbStaticTransforms(const std::shared_ptr& no REGULAR_COUT << GREEN_START << " Initializing smb static transforms..." << COLOR_END << std::endl; } -void SmbStaticTransforms::findTransformations() { +bool SmbStaticTransforms::findTransformations() { // Super Method - graph_msf::StaticTransformsTf::findTransformations(); + // Need to find the transformations in the TF-tree + if (!graph_msf::StaticTransformsTf::findTransformations()) { + REGULAR_COUT << RED_START << " Failed to find transformations in TF-tree." << COLOR_END << std::endl; + return false; + } // Print to console REGULAR_COUT << COLOR_END << " Looking up transforms in TF-tree." << std::endl; - REGULAR_COUT << COLOR_END << " Transforms between the following frames are required:" << std::endl; REGULAR_COUT << COLOR_END << " Waiting for up to 100 seconds until they arrive..." << std::endl; // Temporary variable @@ -38,7 +41,7 @@ void SmbStaticTransforms::findTransformations() { // Imu to LiDAR Link --- if (useLioOdometryFlag_) { - REGULAR_COUT << COLOR_END << " Waiting for transform LO for 10 seconds." << std::endl; + REGULAR_COUT << COLOR_END << " Waiting for transform between " << imuFrame_ << " and " << lidarOdometryFrame_ << " for 10 seconds." << std::endl; transform = tf_buffer_->lookupTransform(imuFrame_, lidarOdometryFrame_, tf2::TimePointZero, tf2::durationFromSec(1.0)); eigenTransform = tf2::transformToEigen(transform.transform); lv_T_frame1_frame2(imuFrame_, lidarOdometryFrame_) = eigenTransform; @@ -50,7 +53,7 @@ void SmbStaticTransforms::findTransformations() { // Imu to VIO Link --- if (useVioOdometryFlag_) { - REGULAR_COUT << COLOR_END << " Waiting for transform VIO for 10 seconds." << std::endl; + REGULAR_COUT << COLOR_END << " Waiting for transform between " << imuFrame_ << " and " << vioOdometryFrame_ << " for 10 seconds." << std::endl; transform = tf_buffer_->lookupTransform(imuFrame_, vioOdometryFrame_, tf2::TimePointZero, tf2::durationFromSec(1.0)); eigenTransform = tf2::transformToEigen(transform.transform); lv_T_frame1_frame2(imuFrame_, vioOdometryFrame_) = eigenTransform; @@ -93,6 +96,7 @@ void SmbStaticTransforms::findTransformations() { } REGULAR_COUT << GREEN_START << " Transforms looked up successfully." << COLOR_END << std::endl; + return true; } } // namespace smb_se diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2bu.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2bu.h deleted file mode 100644 index 69f012cf..00000000 --- a/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2bu.h +++ /dev/null @@ -1,176 +0,0 @@ -/* -Copyright 2024 by Julian Nubert, Robotic Systems Lab, ETH Zurich. -All rights reserved. -This file is released under the "BSD-3-Clause License". -Please see the LICENSE file that has been included as part of this package. - */ - -#pragma once -// std -#include -#include -#include -#include - -// ROS 2 -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Workspace -#include "graph_msf/interface/GraphMsf.h" -#include "graph_msf/interface/GraphMsfClassic.h" -#include "graph_msf/interface/GraphMsfHolistic.h" -// #include "graph_msf_ros2/srv/OfflineOptimizationTrigger.hpp" - -// Macros -#define ROS_QUEUE_SIZE 1 - -namespace graph_msf { - -class GraphMsfRos2 : public GraphMsfClassic, public GraphMsfHolistic { - public: - explicit GraphMsfRos2(const std::shared_ptr& node); - ~GraphMsfRos2() override = default; - - // Setup - void setup(const std::shared_ptr& staticTransformsPtr); - - protected: - // Functions that need implementation - virtual void initializePublishers(); - virtual void initializeSubscribers(); - virtual void initializeMessages(); - // virtual void initializeServices(rclcpp::Node& node); - - // Commodity Functions to be shared ----------------------------------- - static void addToPathMsg(const std::shared_ptr& pathPtr, const std::string& frameName, - const rclcpp::Time& stamp, const Eigen::Vector3d& t, int maxBufferLength); - static void addToOdometryMsg( - const std::shared_ptr& msgPtr, const std::string& fixedFrame, - const std::string& movingFrame, const rclcpp::Time& stamp, const Eigen::Isometry3d& T, - const Eigen::Vector3d& W_v_W_F, const Eigen::Vector3d& W_w_W_F, - const Eigen::Matrix& poseCovariance = Eigen::Matrix::Zero(), - const Eigen::Matrix& twistCovariance = Eigen::Matrix::Zero()); - static void addToPoseWithCovarianceStampedMsg( - const std::shared_ptr& msgPtr, const std::string& frameName, - const rclcpp::Time& stamp, const Eigen::Isometry3d& T, - const Eigen::Matrix& transformCovariance = Eigen::Matrix::Zero()); - - static void extractCovariancesFromOptimizedState( - Eigen::Matrix& poseCovarianceRos, Eigen::Matrix& twistCovarianceRos, - const std::shared_ptr& optimizedStateWithCovarianceAndBiasPtr); - - static void createVelocityMarker(const std::string& referenceFrameName, const rclcpp::Time& stamp, - const Eigen::Vector3d& velocity, visualization_msgs::msg::Marker& marker); - void createContactMarker(const std::string& referenceFrameName, const rclcpp::Time& stamp, - const Eigen::Vector3d& position, const std::string& nameSpace, const int id, - visualization_msgs::msg::Marker& marker); - - // Parameter Loading ----------------------------------- - virtual void readParams(const rclcpp::Node& node); - - // Callbacks - virtual void imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuPtr); - - // Services - // bool srvOfflineSmootherOptimizeCallback( - // const std::shared_ptr req, - // std::shared_ptr res); - - // Publishing ----------------------------------- - virtual void publishState( - const std::shared_ptr& integratedNavStatePtr, - const std::shared_ptr& optimizedStateWithCovarianceAndBiasPtr); - void publishNonTimeCriticalData( - const Eigen::Matrix poseCovarianceRos, const Eigen::Matrix twistCovarianceRos, - const Eigen::Vector3d positionVarianceRos, const Eigen::Vector3d orientationVarianceRos, - const std::shared_ptr integratedNavStatePtr, - const std::shared_ptr optimizedStateWithCovarianceAndBiasPtr); - void publishOptimizedStateAndBias( - const std::shared_ptr optimizedStateWithCovarianceAndBiasPtr, - const Eigen::Matrix& poseCovarianceRos, const Eigen::Matrix& twistCovarianceRos); - - void publishTfTreeTransform(const std::string& frameName, const std::string& childFrameName, double timeStamp, - const Eigen::Isometry3d& T_frame_childFrame); - void publishImuOdoms(const std::shared_ptr& preIntegratedNavStatePtr, - const Eigen::Matrix& poseCovarianceRos, - const Eigen::Matrix& twistCovarianceRos) const; - void publishDiagVarianceVectors(const Eigen::Vector3d& posVarianceRos, const Eigen::Vector3d& rotVarianceRos, - const double timeStamp) const; - void publishVelocityMarkers(const std::shared_ptr& navStatePtr) const; - void publishImuPaths(const std::shared_ptr& navStatePtr) const; - void publishAddedImuMeas(const Eigen::Matrix& addedImuMeas, const rclcpp::Time& stamp) const; - - // Measure time - long secondsSinceStart(); - - // Node - std::shared_ptr node_; - - // Time - std::chrono::time_point startTime; - std::chrono::time_point currentTime; - - // Publishers - tf2_ros::TransformBroadcaster tfBroadcaster; - - // Members - std::string referenceFrameAlignedNameId = "_graph_msf_aligned"; - std::string sensorFrameCorrectedNameId = "_graph_msf_corrected"; - std::string optimizationResultLoggingPath = ""; - - private: - // Publishers - rclcpp::Publisher::SharedPtr pubEstOdomImu_; - rclcpp::Publisher::SharedPtr pubEstWorldImu_; - rclcpp::Publisher::SharedPtr pubOptWorldImu_; - rclcpp::Publisher::SharedPtr pubEstWorldPosVariance_; - rclcpp::Publisher::SharedPtr pubEstWorldRotVariance_; - rclcpp::Publisher::SharedPtr pubVelocityMarker_; - rclcpp::Publisher::SharedPtr pubEstOdomImuPath_; - rclcpp::Publisher::SharedPtr pubEstWorldImuPath_; - rclcpp::Publisher::SharedPtr pubOptWorldImuPath_; - rclcpp::Publisher::SharedPtr pubMeasWorldGnssLPath_; - rclcpp::Publisher::SharedPtr pubMeasWorldGnssRPath_; - rclcpp::Publisher::SharedPtr pubMeasWorldLidarPath_; - rclcpp::Publisher::SharedPtr pubAccelBias_; - rclcpp::Publisher::SharedPtr pubGyroBias_; - rclcpp::Publisher::SharedPtr pubAddedImuMeas_; - - std::map::SharedPtr> - pubPoseStampedByTopicMap_; - - // Subscribers - rclcpp::Subscription::SharedPtr subImu_; - - // Messages - std::shared_ptr estOdomImuMsgPtr_; - std::shared_ptr estWorldImuMsgPtr_; - std::shared_ptr optWorldImuMsgPtr_; - std::shared_ptr estWorldPosVarianceMsgPtr_; - std::shared_ptr estWorldRotVarianceMsgPtr_; - std::shared_ptr estOdomImuPathPtr_; - std::shared_ptr estWorldImuPathPtr_; - std::shared_ptr optWorldImuPathPtr_; - std::shared_ptr measWorldLidarPathPtr_; - std::shared_ptr accelBiasMsgPtr_; - std::shared_ptr gyroBiasMsgPtr_; - - // Service server - // rclcpp::Service::SharedPtr srvSmootherOptimize_; - - // Last Optimized State Timestamp - double lastOptimizedStateTimestamp_ = 0.0; - - // Mutex - std::mutex rosPublisherMutex_; -}; - -} // namespace graph_msf diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsTf.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsTf.h index dbf247b8..09b482cf 100644 --- a/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsTf.h +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/extrinsics/StaticTransformsTf.h @@ -23,7 +23,7 @@ class StaticTransformsTf : public StaticTransforms { virtual ~StaticTransformsTf() = default; protected: - void findTransformations() override; + bool findTransformations() override; // Members std::shared_ptr tf_buffer_; diff --git a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2bu.cpp b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2bu.cpp deleted file mode 100644 index 0c3b0093..00000000 --- a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2bu.cpp +++ /dev/null @@ -1,167 +0,0 @@ -// Implementation -#include "graph_msf_ros2/GraphMsfRos2.h" - -// ROS 2 -#include -#include - -// Workspace -#include "graph_msf_ros2/constants.h" -#include "graph_msf_ros2/util/conversions.h" - -namespace graph_msf { - -GraphMsfRos2::GraphMsfRos2(const std::shared_ptr& nodePtr) : node_(nodePtr), tfBroadcaster(nodePtr) { - RCLCPP_INFO(node_->get_logger(), "GraphMsfRos2-Constructor called."); - graphConfigPtr_ = std::make_shared(); -} - -void GraphMsfRos2::setup(const std::shared_ptr& staticTransformsPtr) { - RCLCPP_INFO(node_->get_logger(), "GraphMsfRos2-Setup called."); - - if (staticTransformsPtr == nullptr) { - throw std::runtime_error("Static transforms not set. They must be set."); - } - - GraphMsfRos2::readParams(*node_); - GraphMsf::setup(graphConfigPtr_, staticTransformsPtr); - - initializePublishers(); - initializeSubscribers(); - initializeMessages(); - // initializeServices(); - - startTime = std::chrono::high_resolution_clock::now(); - RCLCPP_INFO(node_->get_logger(), "Set up successfully."); -} - -void GraphMsfRos2::initializePublishers() { - RCLCPP_INFO(node_->get_logger(), "Initializing publishers."); - - pubEstOdomImu_ = node_->create_publisher("/graph_msf/est_odometry_odom_imu", ROS_QUEUE_SIZE); - pubEstWorldImu_ = - node_->create_publisher("/graph_msf/est_odometry_world_imu", ROS_QUEUE_SIZE); - pubOptWorldImu_ = - node_->create_publisher("/graph_msf/opt_odometry_world_imu", ROS_QUEUE_SIZE); - - pubEstWorldPosVariance_ = - node_->create_publisher("/graph_msf/est_world_pos_variance", ROS_QUEUE_SIZE); - pubEstWorldRotVariance_ = - node_->create_publisher("/graph_msf/est_world_rot_variance", ROS_QUEUE_SIZE); - - pubVelocityMarker_ = - node_->create_publisher("/graph_msf/velocity_marker", ROS_QUEUE_SIZE); - pubEstOdomImuPath_ = node_->create_publisher("/graph_msf/est_path_odom_imu", ROS_QUEUE_SIZE); - pubEstWorldImuPath_ = node_->create_publisher("/graph_msf/est_path_world_imu", ROS_QUEUE_SIZE); - pubOptWorldImuPath_ = node_->create_publisher("/graph_msf/opt_path_world_imu", ROS_QUEUE_SIZE); - - pubAccelBias_ = node_->create_publisher("/graph_msf/accel_bias", ROS_QUEUE_SIZE); - pubGyroBias_ = node_->create_publisher("/graph_msf/gyro_bias", ROS_QUEUE_SIZE); - pubAddedImuMeas_ = node_->create_publisher("/graph_msf/added_imu_meas", ROS_QUEUE_SIZE); -} - -void GraphMsfRos2::initializeSubscribers() { - RCLCPP_INFO(node_->get_logger(), "Initializing subscribers."); - - subImu_ = node_->create_subscription( - "/imu_topic", ROS_QUEUE_SIZE, std::bind(&GraphMsfRos2::imuCallback, this, std::placeholders::_1)); - RCLCPP_INFO(node_->get_logger(), "Initialized main IMU subscriber with topic: /imu_topic"); -} - -void GraphMsfRos2::initializeMessages() { - RCLCPP_INFO(node_->get_logger(), "Initializing messages."); - - estOdomImuMsgPtr_ = std::make_shared(); - estWorldImuMsgPtr_ = std::make_shared(); - optWorldImuMsgPtr_ = std::make_shared(); - - estWorldPosVarianceMsgPtr_ = std::make_shared(); - estWorldRotVarianceMsgPtr_ = std::make_shared(); - - estOdomImuPathPtr_ = std::make_shared(); - estWorldImuPathPtr_ = std::make_shared(); - optWorldImuPathPtr_ = std::make_shared(); - - accelBiasMsgPtr_ = std::make_shared(); - gyroBiasMsgPtr_ = std::make_shared(); -} - -// void GraphMsfRos2::initializeServices() { -// srvSmootherOptimize_ = node_->create_service( -// "/graph_msf/trigger_offline_optimization", -// std::bind(&GraphMsfRos2::srvOfflineSmootherOptimizeCallback, this, std::placeholders::_1, -// std::placeholders::_2)); -// } - -// bool GraphMsfRos2::srvOfflineSmootherOptimizeCallback( -// const std::shared_ptr req, -// std::shared_ptr res) { -// int maxIterations = req->max_optimization_iterations; - -// if (GraphMsf::optimizeSlowBatchSmoother(maxIterations, optimizationResultLoggingPath)) { -// res->success = true; -// res->message = "Optimization successful."; -// } else { -// res->success = false; -// res->message = "Optimization failed."; -// } -// return true; -// } - -// Main IMU Callback -void GraphMsfRos2::imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuMsgPtr) { - Eigen::Vector3d linearAcc(imuMsgPtr->linear_acceleration.x, imuMsgPtr->linear_acceleration.y, - imuMsgPtr->linear_acceleration.z); - Eigen::Vector3d angularVel(imuMsgPtr->angular_velocity.x, imuMsgPtr->angular_velocity.y, - imuMsgPtr->angular_velocity.z); - Eigen::Matrix addedImuMeasurements; - - std::shared_ptr preIntegratedNavStatePtr = nullptr; - std::shared_ptr optimizedStateWithCovarianceAndBiasPtr = nullptr; - - if (GraphMsf::addCoreImuMeasurementAndGetState( - linearAcc, angularVel, imuMsgPtr->header.stamp.sec + 1e-9 * imuMsgPtr->header.stamp.nanosec, - preIntegratedNavStatePtr, optimizedStateWithCovarianceAndBiasPtr, addedImuMeasurements)) { - if (node_->now().seconds() - preIntegratedNavStatePtr->getTimeK() > 0.5) { - RCLCPP_WARN(node_->get_logger(), "Encountered delay of %f seconds.", - node_->now().seconds() - preIntegratedNavStatePtr->getTimeK()); - } - - this->publishState(preIntegratedNavStatePtr, optimizedStateWithCovarianceAndBiasPtr); - } else if (GraphMsf::isGraphInited()) { - RCLCPP_WARN(node_->get_logger(), "Could not add IMU measurement."); - } -} - -void GraphMsfRos2::publishState( - const std::shared_ptr& integratedNavStatePtr, - const std::shared_ptr& optimizedStateWithCovarianceAndBiasPtr) { - Eigen::Matrix poseCovarianceRos, twistCovarianceRos; - extractCovariancesFromOptimizedState(poseCovarianceRos, twistCovarianceRos, optimizedStateWithCovarianceAndBiasPtr); - - Eigen::Vector3d positionVarianceRos = poseCovarianceRos.block<3, 3>(0, 0).diagonal(); - Eigen::Vector3d orientationVarianceRos = poseCovarianceRos.block<3, 3>(3, 3).diagonal(); - - std::thread publishNonTimeCriticalDataThread(&GraphMsfRos2::publishNonTimeCriticalData, this, poseCovarianceRos, - twistCovarianceRos, positionVarianceRos, orientationVarianceRos, - integratedNavStatePtr, optimizedStateWithCovarianceAndBiasPtr); - publishNonTimeCriticalDataThread.detach(); -} - -void GraphMsfRos2::publishNonTimeCriticalData( - const Eigen::Matrix poseCovarianceRos, const Eigen::Matrix twistCovarianceRos, - const Eigen::Vector3d positionVarianceRos, const Eigen::Vector3d orientationVarianceRos, - const std::shared_ptr integratedNavStatePtr, - const std::shared_ptr optimizedStateWithCovarianceAndBiasPtr) { - std::lock_guard lock(rosPublisherMutex_); - - double timeK = integratedNavStatePtr->getTimeK(); - - publishImuOdoms(integratedNavStatePtr, poseCovarianceRos, twistCovarianceRos); - publishDiagVarianceVectors(positionVarianceRos, orientationVarianceRos, timeK); - publishVelocityMarkers(integratedNavStatePtr); - publishImuPaths(integratedNavStatePtr); - publishOptimizedStateAndBias(optimizedStateWithCovarianceAndBiasPtr, poseCovarianceRos, twistCovarianceRos); -} - -} // namespace graph_msf diff --git a/ros2/graph_msf_ros2/src/lib/StaticTransformsTf.cpp b/ros2/graph_msf_ros2/src/lib/StaticTransformsTf.cpp index bb5a9078..53a9b409 100644 --- a/ros2/graph_msf_ros2/src/lib/StaticTransformsTf.cpp +++ b/ros2/graph_msf_ros2/src/lib/StaticTransformsTf.cpp @@ -27,7 +27,7 @@ StaticTransformsTf::StaticTransformsTf(const rclcpp::Node::SharedPtr& node) RCLCPP_INFO(rclcpp::get_logger("graph_msf"), "StaticTransformsTf - Initializing static transforms..."); } -void StaticTransformsTf::findTransformations() { +bool StaticTransformsTf::findTransformations() { RCLCPP_INFO(rclcpp::get_logger("graph_msf"), "Looking up transforms in TF-tree."); RCLCPP_INFO(rclcpp::get_logger("graph_msf"), "Transforms between the following frames are required: %s, %s", imuFrame_.c_str(), baseLinkFrame_.c_str()); @@ -40,24 +40,22 @@ void StaticTransformsTf::findTransformations() { RCLCPP_INFO(rclcpp::get_logger("graph_msf"), "Looking up transform from %s to %s", imuFrame_.c_str(), baseLinkFrame_.c_str()); try { + REGULAR_COUT << COLOR_END << " Waiting for transform between " << imuFrame_ << " and " << baseLinkFrame_ << " for 10 seconds." << std::endl; transform_stamped = tf_buffer_->lookupTransform(imuFrame_, baseLinkFrame_, rclcpp::Time(0), rclcpp::Duration::from_seconds(100.0)); - tf2::Transform tf_transform; - graph_msf::transformStampedToIsometry3(transform_stamped.transform, lv_T_frame1_frame2(imuFrame_, baseLinkFrame_)); - - // graph_msf::tfToIsometry3(tf_transform, lv_T_frame1_frame2(imuFrame_, baseLinkFrame_)); - // graph_msf::tfToIsometry3(transform, lv_T_frame1_frame2(imuFrame_, baseLinkFrame_)); - // RCLCPP_INFO(rclcpp::get_logger("graph_msf"), "Translation I_Base: %s", - // lv_T_frame1_frame2(imuFrame_, baseLinkFrame_) - // .translation() - // .format(Eigen::IOFormat(Eigen::StreamPrecision)) - // .c_str()); - lv_T_frame1_frame2(baseLinkFrame_, imuFrame_) = lv_T_frame1_frame2(imuFrame_, baseLinkFrame_).inverse(); + Eigen::Isometry3d eigenTransform = tf2::transformToEigen(transform_stamped.transform); + lv_T_frame1_frame2(imuFrame_, baseLinkFrame_) = eigenTransform; + std::cout << YELLOW_START << "Smb-StaticTransforms" << COLOR_END << " Translation I_B: " << imuFrame_ << " " << baseLinkFrame_ + << " " << rv_T_frame1_frame2(imuFrame_, baseLinkFrame_).translation() << std::endl; + lv_T_frame1_frame2(baseLinkFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, baseLinkFrame_).inverse(); } catch (const tf2::TransformException& ex) { RCLCPP_ERROR(rclcpp::get_logger("graph_msf"), "Transform lookup failed: %s", ex.what()); + REGULAR_COUT << RED_START << " Transform lookup failed: " << ex.what() << COLOR_END << std::endl; + return false; } - + // Wrap up RCLCPP_INFO(rclcpp::get_logger("graph_msf"), "Transforms looked up successfully."); + return true; } } // namespace graph_msf From 366c468c1d9e8cd86a3e656fbeb762ec54e4b80d Mon Sep 17 00:00:00 2001 From: Julian Date: Fri, 20 Jun 2025 00:58:43 +0200 Subject: [PATCH 46/60] latest update with outlier filtering and offline optimization service --- .../smb_estimator_graph_ros2/CMakeLists.txt | 5 ++ .../config/smb_specific/smb_graph_params.yaml | 2 +- .../smb_estimator_graph_replay.launch.py | 2 +- .../smb_estimator_graph_ros2/package.xml | 3 ++ .../src/lib/SmbEstimator.cpp | 10 +++- ros2/graph_msf_ros2/CMakeLists.txt | 10 +++- .../include/graph_msf_ros2/GraphMsfRos2.h | 13 +++-- .../graph_msf_ros2/ros/read_ros_params.h | 7 ++- ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp | 54 +++++++++---------- ros2/graph_msf_ros2/src/lib/readParams.cpp | 13 +++++ ros2/graph_msf_ros2_msgs/CMakeLists.txt | 6 ++- ros2/graph_msf_ros2_msgs/package.xml | 1 + .../srv/OfflineOptimizationTrigger.srv | 0 13 files changed, 83 insertions(+), 43 deletions(-) rename ros2/{graph_msf_ros2 => graph_msf_ros2_msgs}/srv/OfflineOptimizationTrigger.srv (100%) diff --git a/ros2/examples/smb_estimator_graph_ros2/CMakeLists.txt b/ros2/examples/smb_estimator_graph_ros2/CMakeLists.txt index 3c138700..11d3d545 100644 --- a/ros2/examples/smb_estimator_graph_ros2/CMakeLists.txt +++ b/ros2/examples/smb_estimator_graph_ros2/CMakeLists.txt @@ -18,6 +18,10 @@ find_package(nav_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(Glog REQUIRED) +find_package(graph_msf_ros2_msgs REQUIRED + COMPONENTS rosidl_generator_c rosidl_typesupport_cpp +) + # Check if the target from graph_msf is already defined if(NOT TARGET graph_msf::graph_msf) @@ -81,6 +85,7 @@ ament_target_dependencies(${PROJECT_NAME} tf2_ros Glog gflags + graph_msf_ros2_msgs ) # Executable diff --git a/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml b/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml index 7216a8c0..566aa380 100644 --- a/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml +++ b/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml @@ -23,7 +23,7 @@ # Noise Parameters noise_params: ## LiDAR - lioPoseUnaryNoiseDensity: [ 0.02, 0.02, 0.02, 0.02, 0.02, 0.02 ] # StdDev, ORDER RPY(rad) - XYZ(meters) + lioPoseUnaryNoiseDensity: [ 0.02, 0.02, 0.02, 0.002, 0.002, 0.002 ] # StdDev, ORDER RPY(rad) - XYZ(meters) ## Wheel wheelPoseBetweenNoiseDensity: [ 0.1, 0.1, 0.1, 0.06, 0.06, 0.06 ] # StdDev, ORDER RPY(rad) - XYZ(meters) wheelLinearVelocitiesNoiseDensity: [ 0.3, 0.3, 0.3 ] # StdDev, ORDER VxVyVz(meters/sec) diff --git a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py index fe716fd9..9cd1f390 100644 --- a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py +++ b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_replay.launch.py @@ -51,7 +51,7 @@ def generate_launch_description(): ), DeclareLaunchArgument( "lidar_odometry_topic_name", - default_value="/mapping/scan2map_odometry", + default_value="/open3d/scan2map_odometry", description="Lidar odometry topic name", ), # C. Parameter files diff --git a/ros2/examples/smb_estimator_graph_ros2/package.xml b/ros2/examples/smb_estimator_graph_ros2/package.xml index 0a2434b5..6d237823 100644 --- a/ros2/examples/smb_estimator_graph_ros2/package.xml +++ b/ros2/examples/smb_estimator_graph_ros2/package.xml @@ -26,6 +26,9 @@ graph_msf glog + + smb_description + ament_cmake_gtest rosbag2 diff --git a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp index 27deb2f3..2d8b0989 100644 --- a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp +++ b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp @@ -148,6 +148,14 @@ void SmbEstimator::imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuPtr) { graph_msf::GraphMsf::initYawAndPosition(unary6DMeasurement); graph_msf::GraphMsf::pretendFirstMeasurementReceived(); } + // Remove if norm is larger than 100 + if (std::sqrt(imuPtr->linear_acceleration.x * imuPtr->linear_acceleration.x + + imuPtr->linear_acceleration.y * imuPtr->linear_acceleration.y + + imuPtr->linear_acceleration.z * imuPtr->linear_acceleration.z) > + 100.0) { + REGULAR_COUT << RED_START << " IMU linear acceleration norm is larger than 100, skipping this measurement." << COLOR_END << std::endl; + return; + } graph_msf::GraphMsfRos2::imuCallback(imuPtr); } @@ -178,7 +186,7 @@ void SmbEstimator::lidarOdometryCallback_(const nav_msgs::msg::Odometry::ConstSh graph_msf::UnaryMeasurementXDAbsolute unary6DMeasurement( "Lidar_unary_6D", int(lioOdometryRate_), lioOdometryFrame, lioOdometryFrame + sensorFrameCorrectedNameId, - graph_msf::RobustNorm::None(), lidarOdometryTimeK, 1.0, lio_T_M_Lk, lioPoseUnaryNoise_, odomLidarPtr->header.frame_id, + graph_msf::RobustNorm::Huber(3.0), lidarOdometryTimeK, 1.0, lio_T_M_Lk, lioPoseUnaryNoise_, odomLidarPtr->header.frame_id, staticTransformsPtr_->getWorldFrame(), initialSe3AlignmentNoise_, lioSe3AlignmentRandomWalk_); if (lidarOdometryCallbackCounter__ <= 2) { diff --git a/ros2/graph_msf_ros2/CMakeLists.txt b/ros2/graph_msf_ros2/CMakeLists.txt index be64a6bf..efc150f4 100644 --- a/ros2/graph_msf_ros2/CMakeLists.txt +++ b/ros2/graph_msf_ros2/CMakeLists.txt @@ -18,6 +18,10 @@ find_package(rosidl_default_generators REQUIRED) find_package(tf2_eigen REQUIRED) find_package(std_srvs REQUIRED) find_package(visualization_msgs REQUIRED) +find_package(graph_msf_ros2_msgs REQUIRED + COMPONENTS rosidl_generator_c rosidl_typesupport_cpp +) + # Check if the target from graph_msf is already defined if(NOT TARGET graph_msf::graph_msf) @@ -36,12 +40,12 @@ endif () ## Build ## ########### -# Include directories +# Include directories --------------------------------------------------- include_directories( include ) -# Library +# Library ---------------------------------------------------------------- add_library(${PROJECT_NAME} SHARED src/lib/StaticTransformsTf.cpp src/lib/GraphMsfRos2.cpp @@ -61,6 +65,7 @@ ament_target_dependencies(${PROJECT_NAME} std_srvs visualization_msgs rosidl_default_runtime + graph_msf_ros2_msgs ) target_link_libraries(${PROJECT_NAME} PUBLIC @@ -152,6 +157,7 @@ ament_export_dependencies( std_srvs visualization_msgs rosidl_default_runtime + graph_msf_ros2_msgs ) ament_package() diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h index fdbc878a..b2ea5cfe 100644 --- a/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h @@ -22,7 +22,7 @@ #include "graph_msf/interface/GraphMsf.h" #include "graph_msf/interface/GraphMsfClassic.h" #include "graph_msf/interface/GraphMsfHolistic.h" -// #include "graph_msf_ros/srv/offline_optimization_trigger.hpp" +#include "graph_msf_ros2_msgs/srv/offline_optimization_trigger.hpp" // Macros #define ROS_QUEUE_SIZE 1 @@ -43,7 +43,7 @@ class GraphMsfRos2 : public GraphMsfClassic, public GraphMsfHolistic { virtual void initializePublishers(); virtual void initializeSubscribers(); virtual void initializeMessages(); - // virtual void initializeServices(rclcpp::Node& node); + virtual void initializeServices(rclcpp::Node& node); // Commodity Functions to be shared ----------------------------------- // Static @@ -75,10 +75,9 @@ class GraphMsfRos2 : public GraphMsfClassic, public GraphMsfHolistic { virtual void imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuPtr); // Services - // bool srvOfflineSmootherOptimizeCallback(const - // std::shared_ptr req, - // std::shared_ptr - // res); + bool srvOfflineSmootherOptimizeCallback( + const std::shared_ptr req, + std::shared_ptr res); // Publishing ----------------------------------- virtual void publishState( @@ -164,7 +163,7 @@ class GraphMsfRos2 : public GraphMsfClassic, public GraphMsfHolistic { geometry_msgs::msg::Vector3Stamped::SharedPtr gyroBiasMsgPtr_; // Services - // rclcpp::Service::SharedPtr srvSmootherOptimize_; + rclcpp::Service::SharedPtr srvSmootherOptimize_; // Last Optimized State Timestamp double lastOptimizedStateTimestamp_ = 0.0; diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/ros/read_ros_params.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/ros/read_ros_params.h index 8b244b55..f15fe1dc 100644 --- a/ros2/graph_msf_ros2/include/graph_msf_ros2/ros/read_ros_params.h +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/ros/read_ros_params.h @@ -8,6 +8,7 @@ // Workspace #include "graph_msf/interface/Terminal.h" +#include "graph_msf_ros2/constants.h" namespace graph_msf { @@ -32,13 +33,15 @@ T tryGetParam(std::shared_ptr node, const std::string& key) { if (node->get_parameter(key, value)) { printKey(key, value); - // RCLCPP_INFO(node->get_logger(), "Successfully retrieved parameter: %s", key.c_str()); + RCLCPP_INFO(node->get_logger(), "Successfully retrieved parameter: %s", key.c_str()); + // std::cout << YELLOW_START << "GraphMsfRos2 " << COLOR_END << key.c_str() << " set to: " << value << std::endl; return value; } if (node->get_parameter("/" + key, value)) { printKey("/" + key, value); - // RCLCPP_INFO(node->get_logger(), "Successfully retrieved parameter with absolute key: %s", ("/" + key).c_str()); + RCLCPP_INFO(node->get_logger(), "Successfully retrieved parameter with absolute key: %s", ("/" + key).c_str()); + // std::cout << YELLOW_START << "GraphMsfRos2 " << COLOR_END << key.c_str() << " set to: " << value << std::endl; return value; } diff --git a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp index ece2ad56..6fdfe65f 100644 --- a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp +++ b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp @@ -135,7 +135,7 @@ void GraphMsfRos2::setup(std::shared_ptr staticTransformsPtr) tfBroadcaster_ = std::make_shared(node_); // Services ---------------------------- - // GraphMsfRos::initializeServices(*node_); + GraphMsfRos2::initializeServices(*node_); // Time startTime = std::chrono::high_resolution_clock::now(); @@ -211,32 +211,32 @@ void GraphMsfRos2::initializeMessages() { gyroBiasMsgPtr_ = std::make_shared(); } -// void GraphMsfRos2::initializeServices(rclcpp::Node& node) { -// RCLCPP_INFO(node.get_logger(), "Initializing services."); - -// // Trigger offline smoother optimization -// srvSmootherOptimize_ = node.create_service( -// "/graph_msf/trigger_offline_optimization", -// std::bind(&GraphMsfRos::srvOfflineSmootherOptimizeCallback, this, std::placeholders::_1, -// std::placeholders::_2)); -// } - -// bool GraphMsfRos2::srvOfflineSmootherOptimizeCallback( -// const std::shared_ptr req, -// std::shared_ptr res) { -// // Max Iterations from service call -// int maxIterations = req->max_optimization_iterations; - -// // Trigger offline smoother optimization and create response -// if (GraphMsf::optimizeSlowBatchSmoother(maxIterations, optimizationResultLoggingPath)) { -// res->success = true; -// res->message = "Optimization successful."; -// } else { -// res->success = false; -// res->message = "Optimization failed."; -// } -// return true; -// } +void GraphMsfRos2::initializeServices(rclcpp::Node& node) { + RCLCPP_INFO(node.get_logger(), "Initializing services."); + + // Trigger offline smoother optimization + srvSmootherOptimize_ = node.create_service( + "/graph_msf/trigger_offline_optimization", + std::bind(&GraphMsfRos2::srvOfflineSmootherOptimizeCallback, this, std::placeholders::_1, + std::placeholders::_2)); +} + +bool GraphMsfRos2::srvOfflineSmootherOptimizeCallback( + const std::shared_ptr req, + std::shared_ptr res) { + // Max Iterations from service call + int maxIterations = req->max_optimization_iterations; + + // Trigger offline smoother optimization and create response + if (GraphMsf::optimizeSlowBatchSmoother(maxIterations, optimizationResultLoggingPath, false)) { + res->success = true; + res->message = "Optimization successful."; + } else { + res->success = false; + res->message = "Optimization failed."; + } + return true; +} void GraphMsfRos2::addToPathMsg(const nav_msgs::msg::Path::SharedPtr& pathPtr, const std::string& fixedFrameName, const rclcpp::Time& stamp, const Eigen::Vector3d& t, const int maxBufferLength) { diff --git a/ros2/graph_msf_ros2/src/lib/readParams.cpp b/ros2/graph_msf_ros2/src/lib/readParams.cpp index 51c7685e..9b4275aa 100644 --- a/ros2/graph_msf_ros2/src/lib/readParams.cpp +++ b/ros2/graph_msf_ros2/src/lib/readParams.cpp @@ -13,6 +13,8 @@ Please see the LICENSE file that has been included as part of node_ package. // Workspace #include "graph_msf_ros2/ros/read_ros_params.h" +// Constants +#include "graph_msf_ros2/constants.h" namespace graph_msf { @@ -160,6 +162,17 @@ void GraphMsfRos2::readParams() { if (!std::filesystem::exists(optimizationResultLoggingPath)) { std::filesystem::create_directories(optimizationResultLoggingPath); } + + // TODO: Remove hard coded path + optimizationResultLoggingPath = "/smb_ros2_workspace/logging/"; + + // Print the logging path + std::cout << "-------------------------------------------------" << std::endl; + RCLCPP_INFO(node_->get_logger(), "Optimization results will be logged to: %s", + optimizationResultLoggingPath.c_str()); + REGULAR_COUT << GREEN_START << " Optimization results will be logged to: " + << optimizationResultLoggingPath << COLOR_END << std::endl; + std::cout << "-------------------------------------------------" << std::endl; } } diff --git a/ros2/graph_msf_ros2_msgs/CMakeLists.txt b/ros2/graph_msf_ros2_msgs/CMakeLists.txt index 2f6e3df2..db7157fb 100644 --- a/ros2/graph_msf_ros2_msgs/CMakeLists.txt +++ b/ros2/graph_msf_ros2_msgs/CMakeLists.txt @@ -5,14 +5,16 @@ project(graph_msf_ros2_msgs) find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(nav_msgs REQUIRED) +find_package(std_msgs REQUIRED) # Message and Service Files rosidl_generate_interfaces(${PROJECT_NAME} "msg/Wgs84Coordinate.msg" "srv/GetPathInEnu.srv" - DEPENDENCIES nav_msgs + "srv/OfflineOptimizationTrigger.srv" + DEPENDENCIES nav_msgs std_msgs ) # Package export and installation -ament_export_dependencies(rosidl_default_runtime nav_msgs) +ament_export_dependencies(rosidl_default_runtime nav_msgs std_msgs) ament_package() diff --git a/ros2/graph_msf_ros2_msgs/package.xml b/ros2/graph_msf_ros2_msgs/package.xml index 0b4b3276..303a3450 100644 --- a/ros2/graph_msf_ros2_msgs/package.xml +++ b/ros2/graph_msf_ros2_msgs/package.xml @@ -12,6 +12,7 @@ ament_cmake rosidl_default_generators + rosidl_default_generators rosidl_default_runtime nav_msgs diff --git a/ros2/graph_msf_ros2/srv/OfflineOptimizationTrigger.srv b/ros2/graph_msf_ros2_msgs/srv/OfflineOptimizationTrigger.srv similarity index 100% rename from ros2/graph_msf_ros2/srv/OfflineOptimizationTrigger.srv rename to ros2/graph_msf_ros2_msgs/srv/OfflineOptimizationTrigger.srv From 781149de5edf04b59cc2027b4fc27750e48eb51d Mon Sep 17 00:00:00 2001 From: William Talbot Date: Fri, 20 Jun 2025 17:08:03 +0200 Subject: [PATCH 47/60] fixed ordering of param reading and flag setting for static tfs --- .../src/lib/SmbEstimator.cpp | 14 +++----------- .../src/lib/readParams.cpp | 4 ++++ 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp index 2d8b0989..9bc52bd9 100644 --- a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp +++ b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp @@ -22,17 +22,6 @@ namespace smb_se { SmbEstimator::SmbEstimator(std::shared_ptr& node) : graph_msf::GraphMsfRos2(node) { REGULAR_COUT << GREEN_START << " SmbEstimator-Constructor called." << COLOR_END << std::endl; - staticTransformsPtr_ = std::make_shared(node_); - - // Set the odometry flags in SmbStaticTransforms - auto smbStaticTransforms = std::dynamic_pointer_cast(staticTransformsPtr_); - if (smbStaticTransforms) { - smbStaticTransforms->setUseLioOdometryFlag(useLioOdometryFlag_); - smbStaticTransforms->setUseVioOdometryFlag(useVioOdometryFlag_); - smbStaticTransforms->setUseWheelOdometryBetweenFlag(useWheelOdometryBetweenFlag_); - smbStaticTransforms->setUseWheelLinearVelocitiesFlag(useWheelLinearVelocitiesFlag_); - } - // Call setup after declaring parameters SmbEstimator::setup(); } @@ -72,6 +61,9 @@ void SmbEstimator::setup() { // Wheel Radius (double) node_->declare_parameter("sensor_params.wheelRadius", 0.0); + // Create SmbStaticTransforms + staticTransformsPtr_ = std::make_shared(node_); + SmbEstimator::readParams(); // Initialize ROS 2 publishers and subscribers diff --git a/ros2/examples/smb_estimator_graph_ros2/src/lib/readParams.cpp b/ros2/examples/smb_estimator_graph_ros2/src/lib/readParams.cpp index 7064d0e6..e6ce7b41 100644 --- a/ros2/examples/smb_estimator_graph_ros2/src/lib/readParams.cpp +++ b/ros2/examples/smb_estimator_graph_ros2/src/lib/readParams.cpp @@ -25,9 +25,13 @@ void SmbEstimator::readParams() { // Flags useLioOdometryFlag_ = graph_msf::tryGetParam(node_, "sensor_params.useLioOdometry"); + dynamic_cast(staticTransformsPtr_.get())->setUseLioOdometryFlag(useLioOdometryFlag_); useWheelOdometryBetweenFlag_ = graph_msf::tryGetParam(node_, "sensor_params.useWheelOdometryBetween"); + dynamic_cast(staticTransformsPtr_.get())->setUseWheelOdometryBetweenFlag(useWheelOdometryBetweenFlag_); useWheelLinearVelocitiesFlag_ = graph_msf::tryGetParam(node_, "sensor_params.useWheelLinearVelocities"); + dynamic_cast(staticTransformsPtr_.get())->setUseWheelLinearVelocitiesFlag(useWheelLinearVelocitiesFlag_); useVioOdometryFlag_ = graph_msf::tryGetParam(node_, "sensor_params.useVioOdometry"); + dynamic_cast(staticTransformsPtr_.get())->setUseVioOdometryFlag(useVioOdometryFlag_); // Sensor Params lioOdometryRate_ = graph_msf::tryGetParam(node_, "sensor_params.lioOdometryRate"); From 748c0f18632dabe730d3e0813d35333d7230caa8 Mon Sep 17 00:00:00 2001 From: RobotX-SMB Date: Sat, 21 Jun 2025 01:16:48 +0200 Subject: [PATCH 48/60] Update lidar odometry topic name in smb_estimatior_launch file --- .../launch/smb_estimator_graph.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py index c48c0606..5e70f0a9 100644 --- a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py +++ b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py @@ -51,7 +51,7 @@ def generate_launch_description(): ), DeclareLaunchArgument( "lidar_odometry_topic_name", - default_value="/mapping/scan2map_odometry", + default_value="/open3d/scan2map_odometry", description="Lidar odometry topic name", ), DeclareLaunchArgument( From 6ecfe00efc61be2a51fe4afa71ffc34dd62411e3 Mon Sep 17 00:00:00 2001 From: William Talbot Date: Sat, 21 Jun 2025 12:43:08 +0200 Subject: [PATCH 49/60] added more checks to IMU --- .../smb_estimator_graph_ros2/SmbEstimator.h | 1 + .../src/lib/SmbEstimator.cpp | 27 +++++++++++++++---- 2 files changed, 23 insertions(+), 5 deletions(-) diff --git a/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbEstimator.h b/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbEstimator.h index 12b640dc..3aa408c6 100644 --- a/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbEstimator.h +++ b/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbEstimator.h @@ -90,6 +90,7 @@ class SmbEstimator : public graph_msf::GraphMsfRos2 { int wheelOdometryCallbackCounter_ = -1; Eigen::Isometry3d T_O_Bw_km1_; double wheelOdometryTimeKm1_ = 0.0; + rclcpp::Time last_imu_timestamp = rclcpp::Time(0, 0, RCL_ROS_TIME); // Subscribers rclcpp::Subscription::SharedPtr subLioOdometry_; diff --git a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp index 9bc52bd9..665fcc87 100644 --- a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp +++ b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp @@ -141,13 +141,30 @@ void SmbEstimator::imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuPtr) { graph_msf::GraphMsf::pretendFirstMeasurementReceived(); } // Remove if norm is larger than 100 - if (std::sqrt(imuPtr->linear_acceleration.x * imuPtr->linear_acceleration.x + + const double angular_velocity_norm = std::sqrt(imuPtr->angular_velocity.x * imuPtr->angular_velocity.x + + imuPtr->angular_velocity.y * imuPtr->angular_velocity.y + + imuPtr->angular_velocity.z * imuPtr->angular_velocity.z); + const double linear_acceleration_norm = std::sqrt(imuPtr->linear_acceleration.x * imuPtr->linear_acceleration.x + imuPtr->linear_acceleration.y * imuPtr->linear_acceleration.y + - imuPtr->linear_acceleration.z * imuPtr->linear_acceleration.z) > - 100.0) { - REGULAR_COUT << RED_START << " IMU linear acceleration norm is larger than 100, skipping this measurement." << COLOR_END << std::endl; + imuPtr->linear_acceleration.z * imuPtr->linear_acceleration.z); + if (angular_velocity_norm > 10) { + REGULAR_COUT << RED_START << " IMU angular velocity is larger than 10 rad/s, skipping this measurement." << COLOR_END << std::endl; + return; + } else if (linear_acceleration_norm > 100.0) { + REGULAR_COUT << RED_START << " IMU linear acceleration norm is larger than 100 m/s^2, skipping this measurement." << COLOR_END << std::endl; + return; + } + // Check timestamps strictly increase + rclcpp::Time new_imu_timestamp{imuPtr->header.stamp}; + if (new_imu_timestamp == last_imu_timestamp) { + REGULAR_COUT << RED_START << " IMU timestamp " << new_imu_timestamp.seconds() << " was duplicated, skipping this measurement." << COLOR_END << std::endl; + return; + } else if (new_imu_timestamp < last_imu_timestamp) { + REGULAR_COUT << RED_START << " IMU timestamp " << new_imu_timestamp.seconds() << " was before last included IMU measurement " + " at time" << last_imu_timestamp.seconds() << ", skipping this measurement." << COLOR_END << std::endl; return; } + last_imu_timestamp = new_imu_timestamp; graph_msf::GraphMsfRos2::imuCallback(imuPtr); } @@ -291,4 +308,4 @@ void SmbEstimator::vioOdometryCallback_(const nav_msgs::msg::Odometry::ConstShar pubMeasMapVioPath_->publish(*measVio_mapImuPathPtr_); } -} // namespace smb_se \ No newline at end of file +} // namespace smb_se From ef1e346420f0d2104ccd700e52b0125f303a1c09 Mon Sep 17 00:00:00 2001 From: William Talbot Date: Sat, 21 Jun 2025 14:19:36 +0200 Subject: [PATCH 50/60] cleaned up IMU checks, added more (disabled) time checks --- .../smb_estimator_graph_ros2/SmbEstimator.h | 3 +- .../src/lib/SmbEstimator.cpp | 31 ++++++++++++++----- 2 files changed, 25 insertions(+), 9 deletions(-) diff --git a/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbEstimator.h b/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbEstimator.h index 3aa408c6..da8ea593 100644 --- a/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbEstimator.h +++ b/ros2/examples/smb_estimator_graph_ros2/include/smb_estimator_graph_ros2/SmbEstimator.h @@ -90,7 +90,8 @@ class SmbEstimator : public graph_msf::GraphMsfRos2 { int wheelOdometryCallbackCounter_ = -1; Eigen::Isometry3d T_O_Bw_km1_; double wheelOdometryTimeKm1_ = 0.0; - rclcpp::Time last_imu_timestamp = rclcpp::Time(0, 0, RCL_ROS_TIME); + int num_imu_errors_ = 0; + rclcpp::Time last_imu_timestamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); // Subscribers rclcpp::Subscription::SharedPtr subLioOdometry_; diff --git a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp index 665fcc87..467e3d87 100644 --- a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp +++ b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp @@ -126,6 +126,18 @@ void SmbEstimator::initializeServices() { } void SmbEstimator::imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuPtr) { + const rclcpp::Time new_imu_timestamp{imuPtr->header.stamp}; +// const rclcpp::Time arrival_time = this->node_->get_clock()->now(); +// const rclcpp::Duration delay = arrival_time - new_imu_timestamp; +// if (delay < rclcpp::Duration(0, 0)) { +// REGULAR_COUT << RED_START << " IMU message arrived at " << std::fixed << delay.seconds() << " before the message " +// "timestamp. The messages should not be stamped with a time in the future." << COLOR_END << std::endl; +// } else if (delay > rclcpp::Duration(0, 100000000)) { +// REGULAR_COUT << RED_START << " IMU message arrival was delayed by " << std::fixed << delay.seconds() << "." << COLOR_END << std::endl; +// } else { +// REGULAR_COUT << " IMU message arrival was ok. Delay: " << delay.seconds() << "." << COLOR_END << std::endl; +// } + if (graph_msf::GraphMsf::areRollAndPitchInited() && !graph_msf::GraphMsf::areYawAndPositionInited() && !useLioOdometryFlag_ && !useWheelOdometryBetweenFlag_ && !useWheelLinearVelocitiesFlag_ && !useVioOdometryFlag_) { REGULAR_COUT << RED_START << " IMU callback is setting global yaw and position, as no other odometry is available. Initializing..." @@ -148,23 +160,26 @@ void SmbEstimator::imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuPtr) { imuPtr->linear_acceleration.y * imuPtr->linear_acceleration.y + imuPtr->linear_acceleration.z * imuPtr->linear_acceleration.z); if (angular_velocity_norm > 10) { - REGULAR_COUT << RED_START << " IMU angular velocity is larger than 10 rad/s, skipping this measurement." << COLOR_END << std::endl; + ++num_imu_errors_; + REGULAR_COUT << RED_START << " IMU angular velocity is larger than 10 rad/s, skipping this measurement. Total error count = " << num_imu_errors_ << COLOR_END << std::endl; return; } else if (linear_acceleration_norm > 100.0) { - REGULAR_COUT << RED_START << " IMU linear acceleration norm is larger than 100 m/s^2, skipping this measurement." << COLOR_END << std::endl; + ++num_imu_errors_; + REGULAR_COUT << RED_START << " IMU linear acceleration norm is larger than 100 m/s^2, skipping this measurement. Total error count = " << num_imu_errors_ << COLOR_END << std::endl; return; } // Check timestamps strictly increase - rclcpp::Time new_imu_timestamp{imuPtr->header.stamp}; - if (new_imu_timestamp == last_imu_timestamp) { - REGULAR_COUT << RED_START << " IMU timestamp " << new_imu_timestamp.seconds() << " was duplicated, skipping this measurement." << COLOR_END << std::endl; + if (new_imu_timestamp == last_imu_timestamp_) { + ++num_imu_errors_; + REGULAR_COUT << RED_START << " IMU timestamp " << new_imu_timestamp.seconds() << " was duplicated, skipping this measurement. Total error count = " << num_imu_errors_ << COLOR_END << std::endl; return; - } else if (new_imu_timestamp < last_imu_timestamp) { + } else if (new_imu_timestamp < last_imu_timestamp_) { + ++num_imu_errors_; REGULAR_COUT << RED_START << " IMU timestamp " << new_imu_timestamp.seconds() << " was before last included IMU measurement " - " at time" << last_imu_timestamp.seconds() << ", skipping this measurement." << COLOR_END << std::endl; + " at time" << last_imu_timestamp_.seconds() << ", skipping this measurement. Total error count = " << num_imu_errors_ << COLOR_END << std::endl; return; } - last_imu_timestamp = new_imu_timestamp; + last_imu_timestamp_ = new_imu_timestamp; graph_msf::GraphMsfRos2::imuCallback(imuPtr); } From b3e10a07a17b011f3e04cd703ce8c63233aa46ea Mon Sep 17 00:00:00 2001 From: dishtaweera Date: Sun, 22 Jun 2025 20:32:58 +0000 Subject: [PATCH 51/60] Fix graph_msf_ros2 not found error --- ros2/graph_msf_ros2/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/ros2/graph_msf_ros2/CMakeLists.txt b/ros2/graph_msf_ros2/CMakeLists.txt index efc150f4..687c813f 100644 --- a/ros2/graph_msf_ros2/CMakeLists.txt +++ b/ros2/graph_msf_ros2/CMakeLists.txt @@ -146,6 +146,7 @@ install(FILES # Export ament package ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(${PROJECT_NAME}Targets) ament_export_dependencies( geometry_msgs nav_msgs From fb95ae8d7f03be0b79821a76ec7a6889ae95dd7d Mon Sep 17 00:00:00 2001 From: William Talbot Date: Mon, 23 Jun 2025 13:22:12 +0200 Subject: [PATCH 52/60] increased time window for angular velocity in LocalVelocity3 factor --- .../gmsf_expression/GmsfUnaryExpressionLocalVelocity3.h | 4 ++-- .../config/smb_specific/smb_graph_params.yaml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/graph_msf/include/graph_msf/factors/gmsf_expression/GmsfUnaryExpressionLocalVelocity3.h b/graph_msf/include/graph_msf/factors/gmsf_expression/GmsfUnaryExpressionLocalVelocity3.h index 4944b28a..d2723817 100644 --- a/graph_msf/include/graph_msf/factors/gmsf_expression/GmsfUnaryExpressionLocalVelocity3.h +++ b/graph_msf/include/graph_msf/factors/gmsf_expression/GmsfUnaryExpressionLocalVelocity3.h @@ -33,8 +33,8 @@ class GmsfUnaryExpressionLocalVelocity3 final : public GmsfUnaryExpressionLocal< // Check whether we can find an IMU measurement corresponding to the velocity measurement double imuTimestamp; graph_msf::ImuMeasurement imuMeasurement = graph_msf::ImuMeasurement(); - if (!imuBufferPtr->getClosestImuMeasurement(imuTimestamp, imuMeasurement, 0.02, velocityUnaryMeasurementPtr->timeK())) { - REGULAR_COUT << RED_START << "No IMU Measurement (in time interval) found, assuming 0 angular velocity for the factor." << COLOR_END + if (!imuBufferPtr->getClosestImuMeasurement(imuTimestamp, imuMeasurement, 0.1, velocityUnaryMeasurementPtr->timeK())) { // previously 0.02 + REGULAR_COUT << RED_START << " No IMU Measurement (in time interval) found, assuming 0 angular velocity for the factor." << COLOR_END << std::endl; } diff --git a/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml b/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml index 566aa380..36a302cb 100644 --- a/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml +++ b/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml @@ -9,8 +9,8 @@ useVioOdometry: false ## Rates lioOdometryRate: 10 - wheelOdometryBetweenRate: 50 - wheelLinearVelocitiesRate: 50 + wheelOdometryBetweenRate: 100 + wheelLinearVelocitiesRate: 100 vioOdometryRate: 50 # Wheel Radius wheelRadius: 0.195 # meters From a3220ccba0ebce66a2d6d6cd5734e6639986e123 Mon Sep 17 00:00:00 2001 From: William Talbot Date: Mon, 23 Jun 2025 13:48:31 +0200 Subject: [PATCH 53/60] changed topic names --- .../launch/smb_estimator_graph_sim.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_sim.launch.py b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_sim.launch.py index c5f5ef22..4a4b71f9 100644 --- a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_sim.launch.py +++ b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph_sim.launch.py @@ -47,12 +47,12 @@ def generate_launch_description(): ), DeclareLaunchArgument( 'wheel_odometry_topic_name', - default_value='/control/smb_diff_drive/odom', + default_value='/wheel_odometry', description='Wheel odometry topic name' ), DeclareLaunchArgument( 'wheel_velocities_topic_name', - default_value='/control/smb_lowlevel_controller/wheelSpeeds', + default_value='/wheel_velocities', description='Wheel velocities topic name' ), DeclareLaunchArgument( From 7bb79130935eedb64f526c87ce51e0a5c9370525 Mon Sep 17 00:00:00 2001 From: William Talbot Date: Mon, 23 Jun 2025 19:24:20 +0200 Subject: [PATCH 54/60] fixed wheel velocity by removing robust loss, tuning noise values --- .../config/smb_specific/smb_graph_params.yaml | 2 +- .../launch/smb_estimator_graph.launch.py | 4 ++-- .../src/lib/SmbEstimator.cpp | 16 ++++++++++++++-- 3 files changed, 17 insertions(+), 5 deletions(-) diff --git a/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml b/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml index 36a302cb..01da7795 100644 --- a/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml +++ b/ros2/examples/smb_estimator_graph_ros2/config/smb_specific/smb_graph_params.yaml @@ -26,7 +26,7 @@ lioPoseUnaryNoiseDensity: [ 0.02, 0.02, 0.02, 0.002, 0.002, 0.002 ] # StdDev, ORDER RPY(rad) - XYZ(meters) ## Wheel wheelPoseBetweenNoiseDensity: [ 0.1, 0.1, 0.1, 0.06, 0.06, 0.06 ] # StdDev, ORDER RPY(rad) - XYZ(meters) - wheelLinearVelocitiesNoiseDensity: [ 0.3, 0.3, 0.3 ] # StdDev, ORDER VxVyVz(meters/sec) + wheelLinearVelocitiesNoiseDensity: [ 0.2, 0.1, 0.01 ] # StdDev, ORDER VxVyVz(meters/sec) ## VIO vioPoseBetweenNoiseDensity: [ 100.0, 100.0, 100.0, 0.1, 0.1, 0.1 ] # StdDev, ORDER RPY(rad) - XYZ(meters) vioPoseUnaryNoiseDensity: [ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 ] # StdDev, ORDER RPY(rad) - XYZ(meters) diff --git a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py index 5e70f0a9..d17cf9f3 100644 --- a/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py +++ b/ros2/examples/smb_estimator_graph_ros2/launch/smb_estimator_graph.launch.py @@ -56,12 +56,12 @@ def generate_launch_description(): ), DeclareLaunchArgument( "wheel_odometry_topic_name", - default_value="/control/smb_diff_drive/odom", + default_value="/wheel_odometry", description="Wheel odometry topic name", ), DeclareLaunchArgument( "wheel_velocities_topic_name", - default_value="/control/smb_lowlevel_controller/wheelSpeeds", + default_value="/wheel_velocities", description="Wheel velocities topic name", ), DeclareLaunchArgument( diff --git a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp index 467e3d87..641e656b 100644 --- a/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp +++ b/ros2/examples/smb_estimator_graph_ros2/src/lib/SmbEstimator.cpp @@ -292,15 +292,27 @@ void SmbEstimator::wheelLinearVelocitiesCallback_(const std_msgs::msg::Float64Mu graph_msf::GraphMsf::initYawAndPosition(unary6DMeasurement); } } else { + // graph_msf::UnaryMeasurementXD leftWheelLinearVelocityMeasurement( + // "Wheel_linear_velocity_left", int(wheelLinearVelocitiesRate_), wheelLinearVelocityLeftFrame, + // wheelLinearVelocityLeftFrame + sensorFrameCorrectedNameId, graph_msf::RobustNorm::Tukey(1.0), timeK, 1.0, + // Eigen::Vector3d(leftWheelSpeedMs, 0.0, 0.0), wheelLinearVelocitiesNoise_); + // this->addUnaryVelocity3LocalMeasurement(leftWheelLinearVelocityMeasurement); + + // graph_msf::UnaryMeasurementXD rightWheelLinearVelocityMeasurement( + // "Wheel_linear_velocity_right", int(wheelLinearVelocitiesRate_), wheelLinearVelocityRightFrame, + // wheelLinearVelocityRightFrame + sensorFrameCorrectedNameId, graph_msf::RobustNorm::Tukey(1.0), timeK, 1.0, + // Eigen::Vector3d(rightWheelSpeedMs, 0.0, 0.0), wheelLinearVelocitiesNoise_); + // this->addUnaryVelocity3LocalMeasurement(rightWheelLinearVelocityMeasurement); + graph_msf::UnaryMeasurementXD leftWheelLinearVelocityMeasurement( "Wheel_linear_velocity_left", int(wheelLinearVelocitiesRate_), wheelLinearVelocityLeftFrame, - wheelLinearVelocityLeftFrame + sensorFrameCorrectedNameId, graph_msf::RobustNorm::Tukey(1.0), timeK, 1.0, + wheelLinearVelocityLeftFrame + sensorFrameCorrectedNameId, graph_msf::RobustNorm::None(), timeK, 1.0, Eigen::Vector3d(leftWheelSpeedMs, 0.0, 0.0), wheelLinearVelocitiesNoise_); this->addUnaryVelocity3LocalMeasurement(leftWheelLinearVelocityMeasurement); graph_msf::UnaryMeasurementXD rightWheelLinearVelocityMeasurement( "Wheel_linear_velocity_right", int(wheelLinearVelocitiesRate_), wheelLinearVelocityRightFrame, - wheelLinearVelocityRightFrame + sensorFrameCorrectedNameId, graph_msf::RobustNorm::Tukey(1.0), timeK, 1.0, + wheelLinearVelocityRightFrame + sensorFrameCorrectedNameId, graph_msf::RobustNorm::None(), timeK, 1.0, Eigen::Vector3d(rightWheelSpeedMs, 0.0, 0.0), wheelLinearVelocitiesNoise_); this->addUnaryVelocity3LocalMeasurement(rightWheelLinearVelocityMeasurement); } From 1949a74111a06a2d4368a68439a06b9d935ad28c Mon Sep 17 00:00:00 2001 From: Julian Date: Fri, 18 Jul 2025 20:41:02 +0200 Subject: [PATCH 55/60] remove super long path which slows down stuff after a while --- ros2/graph_msf_ros2/CMakeLists.txt | 18 ++++++++++++------ ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp | 4 ++-- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/ros2/graph_msf_ros2/CMakeLists.txt b/ros2/graph_msf_ros2/CMakeLists.txt index 687c813f..1d948b7e 100644 --- a/ros2/graph_msf_ros2/CMakeLists.txt +++ b/ros2/graph_msf_ros2/CMakeLists.txt @@ -18,12 +18,9 @@ find_package(rosidl_default_generators REQUIRED) find_package(tf2_eigen REQUIRED) find_package(std_srvs REQUIRED) find_package(visualization_msgs REQUIRED) -find_package(graph_msf_ros2_msgs REQUIRED - COMPONENTS rosidl_generator_c rosidl_typesupport_cpp -) - -# Check if the target from graph_msf is already defined +# Same Workspace -------------------- +# Graph MSF if(NOT TARGET graph_msf::graph_msf) find_package(graph_msf REQUIRED) endif() @@ -65,7 +62,6 @@ ament_target_dependencies(${PROJECT_NAME} std_srvs visualization_msgs rosidl_default_runtime - graph_msf_ros2_msgs ) target_link_libraries(${PROJECT_NAME} PUBLIC @@ -76,6 +72,16 @@ target_link_libraries(${PROJECT_NAME} tf2_eigen::tf2_eigen graph_msf::graph_msf ) +# Graph MSF ROS2 MSGS +if (NOT TARGET graph_msf_ros2_msgs) + find_package(graph_msf_ros2_msgs REQUIRED + COMPONENTS rosidl_generator_c rosidl_typesupport_cpp + ) + ament_target_dependencies(${PROJECT_NAME} + PUBLIC + graph_msf_ros2_msgs + ) +endif() # Include directories for the library target_include_directories(${PROJECT_NAME} diff --git a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp index 6fdfe65f..a7fd053c 100644 --- a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp +++ b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp @@ -697,13 +697,13 @@ void GraphMsfRos2::publishVelocityMarkers( void GraphMsfRos2::publishImuPaths(const std::shared_ptr& navStatePtr) const { // odom->imu addToPathMsg(estOdomImuPathPtr_, staticTransformsPtr_->getOdomFrame(), rclcpp::Time(navStatePtr->getTimeK() * 1e9), - navStatePtr->getT_O_Ik_gravityAligned().translation(), graphConfigPtr_->imuBufferLength_ * 20); + navStatePtr->getT_O_Ik_gravityAligned().translation(), graphConfigPtr_->imuBufferLength_ * 4); if (pubEstOdomImuPath_->get_subscription_count() > 0) { pubEstOdomImuPath_->publish(*estOdomImuPathPtr_); } // world->imu addToPathMsg(estWorldImuPathPtr_, staticTransformsPtr_->getWorldFrame(), rclcpp::Time(navStatePtr->getTimeK() * 1e9), - navStatePtr->getT_W_Ik().translation(), graphConfigPtr_->imuBufferLength_ * 20); + navStatePtr->getT_W_Ik().translation(), graphConfigPtr_->imuBufferLength_ * 4); if (pubEstWorldImuPath_->get_subscription_count() > 0) { pubEstWorldImuPath_->publish(*estWorldImuPathPtr_); } From 5fc49af24ed2250df798057f753ae47510dcf4c2 Mon Sep 17 00:00:00 2001 From: Julian Date: Tue, 22 Jul 2025 21:49:14 +0200 Subject: [PATCH 56/60] faster update of relative transforms --- .../include/graph_msf_ros2/GraphMsfRos2.h | 5 +- ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp | 146 ++++++++---------- 2 files changed, 69 insertions(+), 82 deletions(-) diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h index b2ea5cfe..3eb5432d 100644 --- a/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h @@ -90,10 +90,10 @@ class GraphMsfRos2 : public GraphMsfClassic, public GraphMsfHolistic { const std::shared_ptr optimizedStateWithCovarianceAndBiasPtr); void publishOptimizedStateAndBias( const std::shared_ptr optimizedStateWithCovarianceAndBiasPtr, - const Eigen::Matrix& poseCovarianceRos, const Eigen::Matrix& twistCovarianceRos); + const Eigen::Matrix& poseCovarianceRos, const Eigen::Matrix& twistCovarianceRos, const double timeStamp); void publishTfTreeTransform(const std::string& frameName, const std::string& childFrameName, double timeStamp, - const Eigen::Isometry3d& T_frame_childFrame); + const Eigen::Isometry3d& T_frame_childFrame) const; void publishImuOdoms(const std::shared_ptr& preIntegratedNavStatePtr, const Eigen::Matrix& poseCovarianceRos, const Eigen::Matrix& twistCovarianceRos) const; @@ -167,6 +167,7 @@ class GraphMsfRos2 : public GraphMsfClassic, public GraphMsfHolistic { // Last Optimized State Timestamp double lastOptimizedStateTimestamp_ = 0.0; + double lastIntegratedStateTimestamp_ = 0.0; // Mutex std::mutex rosPublisherMutex_; diff --git a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp index a7fd053c..923a1944 100644 --- a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp +++ b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp @@ -59,7 +59,7 @@ void GraphMsfRos2::setup(std::shared_ptr staticTransformsPtr) node_->declare_parameter("graph_params.optimizeExtrinsicSensorToSensorCorrectedOffset", false); node_->declare_parameter("graph_params.referenceFramePosesResetThreshold", 0.0); node_->declare_parameter("graph_params.centerMeasurementsAtKeyframePositionBeforeAlignment", false); - + node_->declare_parameter("graph_params.useWindowForMarginalsComputation", false); node_->declare_parameter("graph_params.windowSizeSecondsForMarginalsComputation", 0.0); node_->declare_parameter("graph_params.createReferenceAlignmentKeyframeEveryNSeconds", 0.0); @@ -149,10 +149,8 @@ void GraphMsfRos2::initializePublishers() { // Odometry pubEstOdomImu_ = node_->create_publisher("/graph_msf/est_odometry_odom_imu", ROS_QUEUE_SIZE); - pubEstWorldImu_ = - node_->create_publisher("/graph_msf/est_odometry_world_imu", ROS_QUEUE_SIZE); - pubOptWorldImu_ = - node_->create_publisher("/graph_msf/opt_odometry_world_imu", ROS_QUEUE_SIZE); + pubEstWorldImu_ = node_->create_publisher("/graph_msf/est_odometry_world_imu", ROS_QUEUE_SIZE); + pubOptWorldImu_ = node_->create_publisher("/graph_msf/opt_odometry_world_imu", ROS_QUEUE_SIZE); // Vector3 Variances pubEstWorldPosVariance_ = @@ -161,8 +159,7 @@ void GraphMsfRos2::initializePublishers() { node_->create_publisher("/graph_msf/est_world_rot_variance", ROS_QUEUE_SIZE); // Velocity Marker - pubVelocityMarker_ = - node_->create_publisher("/graph_msf/velocity_marker", ROS_QUEUE_SIZE); + pubVelocityMarker_ = node_->create_publisher("/graph_msf/velocity_marker", ROS_QUEUE_SIZE); // Paths pubEstOdomImuPath_ = node_->create_publisher("/graph_msf/est_path_odom_imu", ROS_QUEUE_SIZE); @@ -181,12 +178,10 @@ void GraphMsfRos2::initializeSubscribers() { RCLCPP_INFO(node_->get_logger(), "Initializing subscribers."); // Imu - subImu_ = node_->create_subscription( - "/imu_topic", rclcpp::QoS(ROS_QUEUE_SIZE).best_effort(), - std::bind(&GraphMsfRos2::imuCallback, this, std::placeholders::_1)); + subImu_ = node_->create_subscription("/imu_topic", rclcpp::QoS(ROS_QUEUE_SIZE).best_effort(), + std::bind(&GraphMsfRos2::imuCallback, this, std::placeholders::_1)); - RCLCPP_INFO(node_->get_logger(), "GraphMsfRos2 Initialized main IMU subscriber with topic: %s", - subImu_->get_topic_name()); + RCLCPP_INFO(node_->get_logger(), "GraphMsfRos2 Initialized main IMU subscriber with topic: %s", subImu_->get_topic_name()); } void GraphMsfRos2::initializeMessages() { @@ -217,12 +212,11 @@ void GraphMsfRos2::initializeServices(rclcpp::Node& node) { // Trigger offline smoother optimization srvSmootherOptimize_ = node.create_service( "/graph_msf/trigger_offline_optimization", - std::bind(&GraphMsfRos2::srvOfflineSmootherOptimizeCallback, this, std::placeholders::_1, - std::placeholders::_2)); + std::bind(&GraphMsfRos2::srvOfflineSmootherOptimizeCallback, this, std::placeholders::_1, std::placeholders::_2)); } bool GraphMsfRos2::srvOfflineSmootherOptimizeCallback( - const std::shared_ptr req, + const std::shared_ptr req, std::shared_ptr res) { // Max Iterations from service call int maxIterations = req->max_optimization_iterations; @@ -238,8 +232,8 @@ bool GraphMsfRos2::srvOfflineSmootherOptimizeCallback( return true; } -void GraphMsfRos2::addToPathMsg(const nav_msgs::msg::Path::SharedPtr& pathPtr, const std::string& fixedFrameName, - const rclcpp::Time& stamp, const Eigen::Vector3d& t, const int maxBufferLength) { +void GraphMsfRos2::addToPathMsg(const nav_msgs::msg::Path::SharedPtr& pathPtr, const std::string& fixedFrameName, const rclcpp::Time& stamp, + const Eigen::Vector3d& t, const int maxBufferLength) { geometry_msgs::msg::PoseStamped pose; pose.header.frame_id = fixedFrameName; pose.header.stamp = stamp; @@ -255,10 +249,9 @@ void GraphMsfRos2::addToPathMsg(const nav_msgs::msg::Path::SharedPtr& pathPtr, c } void GraphMsfRos2::addToOdometryMsg(const nav_msgs::msg::Odometry::SharedPtr& msgPtr, const std::string& fixedFrame, - const std::string& movingFrame, const rclcpp::Time& stamp, - const Eigen::Isometry3d& T, const Eigen::Vector3d& F_v_W_F, - const Eigen::Vector3d& F_w_W_F, const Eigen::Matrix& poseCovariance, - const Eigen::Matrix& twistCovariance) { + const std::string& movingFrame, const rclcpp::Time& stamp, const Eigen::Isometry3d& T, + const Eigen::Vector3d& F_v_W_F, const Eigen::Vector3d& F_w_W_F, + const Eigen::Matrix& poseCovariance, const Eigen::Matrix& twistCovariance) { msgPtr->header.frame_id = fixedFrame; msgPtr->child_frame_id = movingFrame; msgPtr->header.stamp = stamp; @@ -283,9 +276,9 @@ void GraphMsfRos2::addToOdometryMsg(const nav_msgs::msg::Odometry::SharedPtr& ms } } -void GraphMsfRos2::addToPoseWithCovarianceStampedMsg( - const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr& msgPtr, const std::string& frameName, - const rclcpp::Time& stamp, const Eigen::Isometry3d& T, const Eigen::Matrix& transformCovariance) { +void GraphMsfRos2::addToPoseWithCovarianceStampedMsg(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr& msgPtr, + const std::string& frameName, const rclcpp::Time& stamp, const Eigen::Isometry3d& T, + const Eigen::Matrix& transformCovariance) { msgPtr->header.frame_id = frameName; msgPtr->header.stamp = stamp; @@ -306,8 +299,8 @@ void GraphMsfRos2::extractCovariancesFromOptimizedState( const std::shared_ptr& optimizedStateWithCovarianceAndBiasPtr) { // Extract covariances from optimized state if (optimizedStateWithCovarianceAndBiasPtr != nullptr) { - poseCovarianceRos = graph_msf::convertCovarianceGtsamConventionToRosConvention( - optimizedStateWithCovarianceAndBiasPtr->getPoseCovariance()); + poseCovarianceRos = + graph_msf::convertCovarianceGtsamConventionToRosConvention(optimizedStateWithCovarianceAndBiasPtr->getPoseCovariance()); twistCovarianceRos.block<3, 3>(0, 0) = optimizedStateWithCovarianceAndBiasPtr->getVelocityCovariance(); } else { poseCovarianceRos.setZero(); @@ -316,8 +309,8 @@ void GraphMsfRos2::extractCovariancesFromOptimizedState( } // Markers -void GraphMsfRos2::createVelocityMarker(const std::string& referenceFrameName, const rclcpp::Time& stamp, - const Eigen::Vector3d& velocity, visualization_msgs::msg::Marker& marker) { +void GraphMsfRos2::createVelocityMarker(const std::string& referenceFrameName, const rclcpp::Time& stamp, const Eigen::Vector3d& velocity, + visualization_msgs::msg::Marker& marker) { // Arrow marker.header.frame_id = referenceFrameName; marker.header.stamp = stamp; @@ -355,9 +348,8 @@ void GraphMsfRos2::createVelocityMarker(const std::string& referenceFrameName, c marker.pose.orientation.w = q.w(); } -void GraphMsfRos2::createContactMarker(const std::string& referenceFrameName, const rclcpp::Time& stamp, - const Eigen::Vector3d& position, const std::string& nameSpace, const int id, - visualization_msgs::msg::Marker& marker) { +void GraphMsfRos2::createContactMarker(const std::string& referenceFrameName, const rclcpp::Time& stamp, const Eigen::Vector3d& position, + const std::string& nameSpace, const int id, visualization_msgs::msg::Marker& marker) { // Sphere marker.header.frame_id = referenceFrameName; marker.header.stamp = stamp; @@ -392,10 +384,8 @@ long GraphMsfRos2::secondsSinceStart() { // Main IMU Callback void GraphMsfRos2::imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuMsgPtr) { // Convert to Eigen - Eigen::Vector3d linearAcc(imuMsgPtr->linear_acceleration.x, imuMsgPtr->linear_acceleration.y, - imuMsgPtr->linear_acceleration.z); - Eigen::Vector3d angularVel(imuMsgPtr->angular_velocity.x, imuMsgPtr->angular_velocity.y, - imuMsgPtr->angular_velocity.z); + Eigen::Vector3d linearAcc(imuMsgPtr->linear_acceleration.x, imuMsgPtr->linear_acceleration.y, imuMsgPtr->linear_acceleration.z); + Eigen::Vector3d angularVel(imuMsgPtr->angular_velocity.x, imuMsgPtr->angular_velocity.y, imuMsgPtr->angular_velocity.z); Eigen::Matrix addedImuMeasurements; // accel, gyro // Create pointer for carrying state @@ -403,9 +393,9 @@ void GraphMsfRos2::imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuMsgPtr) std::shared_ptr optimizedStateWithCovarianceAndBiasPtr = nullptr; // Add measurement and get state - if (GraphMsf::addCoreImuMeasurementAndGetState( - linearAcc, angularVel, imuMsgPtr->header.stamp.sec + 1e-9 * imuMsgPtr->header.stamp.nanosec, - preIntegratedNavStatePtr, optimizedStateWithCovarianceAndBiasPtr, addedImuMeasurements)) { + if (GraphMsf::addCoreImuMeasurementAndGetState(linearAcc, angularVel, + imuMsgPtr->header.stamp.sec + 1e-9 * imuMsgPtr->header.stamp.nanosec, + preIntegratedNavStatePtr, optimizedStateWithCovarianceAndBiasPtr, addedImuMeasurements)) { // Encountered Delay auto now = clock_->now(); auto delay = now.seconds() - preIntegratedNavStatePtr->getTimeK(); @@ -414,8 +404,7 @@ void GraphMsfRos2::imuCallback(const sensor_msgs::msg::Imu::SharedPtr imuMsgPtr) // Print now vs chrono time auto currentChronoTime = std::chrono::high_resolution_clock::now(); auto chronoTimeSinceEpoch = std::chrono::duration_cast>(currentChronoTime.time_since_epoch()).count(); - RCLCPP_WARN(node_->get_logger(), "Now: %.14f, Std chrono time: %.14f", - now.seconds(), chronoTimeSinceEpoch); + RCLCPP_WARN(node_->get_logger(), "Now: %.14f, Std chrono time: %.14f", now.seconds(), chronoTimeSinceEpoch); } // Publish Odometry @@ -443,9 +432,9 @@ void GraphMsfRos2::publishState( Eigen::Vector3d orientationVarianceRos = poseCovarianceRos.block<3, 3>(3, 3).diagonal(); // Publish non-time critical data in a separate thread - std::thread publishNonTimeCriticalDataThread(&GraphMsfRos2::publishNonTimeCriticalData, this, poseCovarianceRos, - twistCovarianceRos, positionVarianceRos, orientationVarianceRos, - integratedNavStatePtr, optimizedStateWithCovarianceAndBiasPtr); + std::thread publishNonTimeCriticalDataThread(&GraphMsfRos2::publishNonTimeCriticalData, this, poseCovarianceRos, twistCovarianceRos, + positionVarianceRos, orientationVarianceRos, integratedNavStatePtr, + optimizedStateWithCovarianceAndBiasPtr); publishNonTimeCriticalDataThread.detach(); } @@ -466,9 +455,9 @@ void GraphMsfRos2::publishNonTimeCriticalData( // Publish to TF // B_O - Eigen::Isometry3d T_B_Ok = staticTransformsPtr_->rv_T_frame1_frame2(staticTransformsPtr_->getBaseLinkFrame(), - staticTransformsPtr_->getImuFrame()) * - integratedNavStatePtr->getT_O_Ik_gravityAligned().inverse(); + Eigen::Isometry3d T_B_Ok = + staticTransformsPtr_->rv_T_frame1_frame2(staticTransformsPtr_->getBaseLinkFrame(), staticTransformsPtr_->getImuFrame()) * + integratedNavStatePtr->getT_O_Ik_gravityAligned().inverse(); publishTfTreeTransform(staticTransformsPtr_->getBaseLinkFrame(), staticTransformsPtr_->getOdomFrame(), timeK, T_B_Ok); // O_W Eigen::Isometry3d T_O_W = integratedNavStatePtr->getT_W_O().inverse(); @@ -484,12 +473,13 @@ void GraphMsfRos2::publishNonTimeCriticalData( publishImuPaths(integratedNavStatePtr); // Optimized estimate ---------------------- - publishOptimizedStateAndBias(optimizedStateWithCovarianceAndBiasPtr, poseCovarianceRos, twistCovarianceRos); + publishOptimizedStateAndBias(optimizedStateWithCovarianceAndBiasPtr, poseCovarianceRos, twistCovarianceRos, timeK); } void GraphMsfRos2::publishOptimizedStateAndBias( const std::shared_ptr optimizedStateWithCovarianceAndBiasPtr, - const Eigen::Matrix& poseCovarianceRos, const Eigen::Matrix& twistCovarianceRos) { + const Eigen::Matrix& poseCovarianceRos, const Eigen::Matrix& twistCovarianceRos, const double timeStamp) { + // A: Publish Odometry and IMU Biases if (optimizedStateWithCovarianceAndBiasPtr != nullptr && optimizedStateWithCovarianceAndBiasPtr->getTimeK() - lastOptimizedStateTimestamp_ > 1e-03) { // Time of this optimized state @@ -500,8 +490,7 @@ void GraphMsfRos2::publishOptimizedStateAndBias( if (pubOptWorldImu_->get_subscription_count() > 0) { addToOdometryMsg(optWorldImuMsgPtr_, staticTransformsPtr_->getWorldFrame(), staticTransformsPtr_->getImuFrame(), rclcpp::Time(optimizedStateWithCovarianceAndBiasPtr->getTimeK() * 1e9), - optimizedStateWithCovarianceAndBiasPtr->getT_W_Ik(), - optimizedStateWithCovarianceAndBiasPtr->getI_v_W_I(), + optimizedStateWithCovarianceAndBiasPtr->getT_W_Ik(), optimizedStateWithCovarianceAndBiasPtr->getI_v_W_I(), optimizedStateWithCovarianceAndBiasPtr->getI_w_W_I(), poseCovarianceRos, twistCovarianceRos); pubOptWorldImu_->publish(*optWorldImuMsgPtr_); } @@ -510,8 +499,7 @@ void GraphMsfRos2::publishOptimizedStateAndBias( // world->imu addToPathMsg(optWorldImuPathPtr_, staticTransformsPtr_->getWorldFrame(), rclcpp::Time(optimizedStateWithCovarianceAndBiasPtr->getTimeK() * 1e9), - optimizedStateWithCovarianceAndBiasPtr->getT_W_Ik().translation(), - graphConfigPtr_->imuBufferLength_ * 20); + optimizedStateWithCovarianceAndBiasPtr->getT_W_Ik().translation(), graphConfigPtr_->imuBufferLength_ * 20); if (pubOptWorldImuPath_->get_subscription_count() > 0) { pubOptWorldImuPath_->publish(*optWorldImuPathPtr_); } @@ -537,7 +525,11 @@ void GraphMsfRos2::publishOptimizedStateAndBias( gyroBiasMsgPtr_->vector.z = gyroBias(2); pubGyroBias_->publish(*gyroBiasMsgPtr_); } + } // Publishing odometry and biases + // B: Publish Transforms + if (optimizedStateWithCovarianceAndBiasPtr != nullptr && timeStamp - lastIntegratedStateTimestamp_ > 1e-03) { + lastIntegratedStateTimestamp_ = timeStamp; // TFs in Optimized State for (const auto& framePairTransformMapIterator : optimizedStateWithCovarianceAndBiasPtr->getReferenceFrameTransforms().getTransformsMap()) { @@ -560,8 +552,7 @@ void GraphMsfRos2::publishOptimizedStateAndBias( mapFrameName = framePairTransformMapIterator.first.second; // B. Publish TransformStamped for Aligned Frames - std::string transformTopic = - "/graph_msf/transform_" + worldFrameName + "_to_" + mapFrameName + referenceFrameAlignedNameId; + std::string transformTopic = "/graph_msf/transform_" + worldFrameName + "_to_" + mapFrameName + referenceFrameAlignedNameId; auto poseWithCovarianceStampedMsgPtr = std::make_shared(); addToPoseWithCovarianceStampedMsg( poseWithCovarianceStampedMsgPtr, staticTransformsPtr_->getWorldFrame(), @@ -594,10 +585,10 @@ void GraphMsfRos2::publishOptimizedStateAndBias( std::string transformTopic = "/graph_msf/transform_" + sensorFrameName + "_to_" + sensorFrameNameCorrected; auto poseWithCovarianceStampedMsgPtr = std::make_shared(); addToPoseWithCovarianceStampedMsg( - poseWithCovarianceStampedMsgPtr, sensorFrameName, - rclcpp::Time(optimizedStateWithCovarianceAndBiasPtr->getTimeK() * 1e9), T_sensor_sensorCorrected, - optimizedStateWithCovarianceAndBiasPtr->getReferenceFrameTransformsCovariance().rv_T_frame1_frame2( - sensorFrameName, sensorFrameNameCorrected)); + poseWithCovarianceStampedMsgPtr, sensorFrameName, rclcpp::Time(optimizedStateWithCovarianceAndBiasPtr->getTimeK() * 1e9), + T_sensor_sensorCorrected, + optimizedStateWithCovarianceAndBiasPtr->getReferenceFrameTransformsCovariance().rv_T_frame1_frame2(sensorFrameName, + sensorFrameNameCorrected)); // Check whether publisher already exists if (pubPoseStampedByTopicMap_.find(transformTopic) == pubPoseStampedByTopicMap_.end()) { pubPoseStampedByTopicMap_[transformTopic] = @@ -609,16 +600,16 @@ void GraphMsfRos2::publishOptimizedStateAndBias( } // B. Publish TF Tree - publishTfTreeTransform(sensorFrameName, sensorFrameNameCorrected, - optimizedStateWithCovarianceAndBiasPtr->getTimeK(), T_sensor_sensorCorrected); + publishTfTreeTransform(sensorFrameName, sensorFrameNameCorrected, optimizedStateWithCovarianceAndBiasPtr->getTimeK(), + T_sensor_sensorCorrected); } } - } + } // Publishing of Transforms } // Lower Level Functions -void GraphMsfRos2::publishTfTreeTransform(const std::string& parentFrameName, const std::string& childFrameName, - const double timeStamp, const Eigen::Isometry3d& T_frame_childFrame) { +void GraphMsfRos2::publishTfTreeTransform(const std::string& parentFrameName, const std::string& childFrameName, const double timeStamp, + const Eigen::Isometry3d& T_frame_childFrame) const { geometry_msgs::msg::TransformStamped transformStamped; transformStamped.header.stamp = rclcpp::Time(timeStamp * 1e9); transformStamped.header.frame_id = parentFrameName; @@ -638,29 +629,27 @@ void GraphMsfRos2::publishTfTreeTransform(const std::string& parentFrameName, co tfBroadcaster_->sendTransform(transformStamped); } -void GraphMsfRos2::publishImuOdoms( - const std::shared_ptr& preIntegratedNavStatePtr, - const Eigen::Matrix& poseCovarianceRos, const Eigen::Matrix& twistCovarianceRos) const { +void GraphMsfRos2::publishImuOdoms(const std::shared_ptr& preIntegratedNavStatePtr, + const Eigen::Matrix& poseCovarianceRos, + const Eigen::Matrix& twistCovarianceRos) const { // Odom->imu if (pubEstOdomImu_->get_subscription_count() > 0) { addToOdometryMsg(estOdomImuMsgPtr_, staticTransformsPtr_->getOdomFrame(), staticTransformsPtr_->getImuFrame(), - rclcpp::Time(preIntegratedNavStatePtr->getTimeK() * 1e9), - preIntegratedNavStatePtr->getT_O_Ik_gravityAligned(), preIntegratedNavStatePtr->getI_v_W_I(), - preIntegratedNavStatePtr->getI_w_W_I(), poseCovarianceRos, twistCovarianceRos); + rclcpp::Time(preIntegratedNavStatePtr->getTimeK() * 1e9), preIntegratedNavStatePtr->getT_O_Ik_gravityAligned(), + preIntegratedNavStatePtr->getI_v_W_I(), preIntegratedNavStatePtr->getI_w_W_I(), poseCovarianceRos, twistCovarianceRos); pubEstOdomImu_->publish(*estOdomImuMsgPtr_); } // World->imu if (pubEstWorldImu_->get_subscription_count() > 0) { addToOdometryMsg(estWorldImuMsgPtr_, staticTransformsPtr_->getWorldFrame(), staticTransformsPtr_->getImuFrame(), rclcpp::Time(preIntegratedNavStatePtr->getTimeK() * 1e9), preIntegratedNavStatePtr->getT_W_Ik(), - preIntegratedNavStatePtr->getI_v_W_I(), preIntegratedNavStatePtr->getI_w_W_I(), poseCovarianceRos, - twistCovarianceRos); + preIntegratedNavStatePtr->getI_v_W_I(), preIntegratedNavStatePtr->getI_w_W_I(), poseCovarianceRos, twistCovarianceRos); pubEstWorldImu_->publish(*estWorldImuMsgPtr_); } } -void GraphMsfRos2::publishDiagVarianceVectors(const Eigen::Vector3d& posVarianceRos, - const Eigen::Vector3d& rotVarianceRos, const double timeStamp) const { +void GraphMsfRos2::publishDiagVarianceVectors(const Eigen::Vector3d& posVarianceRos, const Eigen::Vector3d& rotVarianceRos, + const double timeStamp) const { // World Position Variance if (pubEstWorldPosVariance_->get_subscription_count() > 0) { estWorldPosVarianceMsgPtr_->header.stamp = rclcpp::Time(timeStamp * 1e9); @@ -681,12 +670,11 @@ void GraphMsfRos2::publishDiagVarianceVectors(const Eigen::Vector3d& posVariance } } -void GraphMsfRos2::publishVelocityMarkers( - const std::shared_ptr& navStatePtr) const { +void GraphMsfRos2::publishVelocityMarkers(const std::shared_ptr& navStatePtr) const { // Velocity in Odom Frame Marker visualization_msgs::msg::Marker velocityMarker; - createVelocityMarker(staticTransformsPtr_->getImuFrame(), rclcpp::Time(navStatePtr->getTimeK() * 1e9), - navStatePtr->getI_v_W_I(), velocityMarker); + createVelocityMarker(staticTransformsPtr_->getImuFrame(), rclcpp::Time(navStatePtr->getTimeK() * 1e9), navStatePtr->getI_v_W_I(), + velocityMarker); // Publish if (pubVelocityMarker_->get_subscription_count() > 0) { @@ -709,8 +697,7 @@ void GraphMsfRos2::publishImuPaths(const std::shared_ptr& addedImuMeas, - const rclcpp::Time& stamp) const { +void GraphMsfRos2::publishAddedImuMeas(const Eigen::Matrix& addedImuMeas, const rclcpp::Time& stamp) const { // Publish added imu measurement if (pubAddedImuMeas_->get_subscription_count() > 0) { sensor_msgs::msg::Imu addedImuMeasMsg; @@ -727,4 +714,3 @@ void GraphMsfRos2::publishAddedImuMeas(const Eigen::Matrix& addedI } } // namespace graph_msf - From ff757eaa6ca67cee838319bee93f945dc2bdcc95 Mon Sep 17 00:00:00 2001 From: Julian Date: Wed, 23 Jul 2025 21:02:48 +0200 Subject: [PATCH 57/60] trying to get rid of ament message generation, for now reverting to standard services as a fallback --- ros2/graph_msf_ros2/CMakeLists.txt | 13 +- .../include/graph_msf_ros2/GraphMsfRos2.h | 7 +- ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp | 13 +- ros2/graph_msf_ros2_msgs/CMakeLists.txt | 75 ++++- .../graph_msf_ros2_msgsConfig.cmake.in | 12 + ...sidl_generator_cpp__visibility_control.hpp | 42 +++ .../offline_optimization_trigger__builder.hpp | 114 ++++++++ .../offline_optimization_trigger__struct.hpp | 275 ++++++++++++++++++ .../offline_optimization_trigger__traits.hpp | 273 +++++++++++++++++ ...ine_optimization_trigger__type_support.hpp | 71 +++++ .../srv/offline_optimization_trigger.hpp | 12 + 11 files changed, 886 insertions(+), 21 deletions(-) create mode 100644 ros2/graph_msf_ros2_msgs/graph_msf_ros2_msgsConfig.cmake.in create mode 100644 ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/msg/rosidl_generator_cpp__visibility_control.hpp create mode 100644 ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__builder.hpp create mode 100644 ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__struct.hpp create mode 100644 ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__traits.hpp create mode 100644 ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__type_support.hpp create mode 100644 ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/offline_optimization_trigger.hpp diff --git a/ros2/graph_msf_ros2/CMakeLists.txt b/ros2/graph_msf_ros2/CMakeLists.txt index 1d948b7e..a69811f2 100644 --- a/ros2/graph_msf_ros2/CMakeLists.txt +++ b/ros2/graph_msf_ros2/CMakeLists.txt @@ -71,17 +71,10 @@ target_link_libraries(${PROJECT_NAME} tf2_ros::tf2_ros tf2_eigen::tf2_eigen graph_msf::graph_msf + # graph_msf_ros2_msgs::graph_msf_ros2_msgs + # rosidl_typesupport_cpp::rosidl_typesupport_cpp + # rosidl_service_type_support_t::rosidl_service_type_support_t ) -# Graph MSF ROS2 MSGS -if (NOT TARGET graph_msf_ros2_msgs) - find_package(graph_msf_ros2_msgs REQUIRED - COMPONENTS rosidl_generator_c rosidl_typesupport_cpp - ) - ament_target_dependencies(${PROJECT_NAME} - PUBLIC - graph_msf_ros2_msgs - ) -endif() # Include directories for the library target_include_directories(${PROJECT_NAME} diff --git a/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h b/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h index 3eb5432d..90b56102 100644 --- a/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h +++ b/ros2/graph_msf_ros2/include/graph_msf_ros2/GraphMsfRos2.h @@ -22,7 +22,6 @@ #include "graph_msf/interface/GraphMsf.h" #include "graph_msf/interface/GraphMsfClassic.h" #include "graph_msf/interface/GraphMsfHolistic.h" -#include "graph_msf_ros2_msgs/srv/offline_optimization_trigger.hpp" // Macros #define ROS_QUEUE_SIZE 1 @@ -76,8 +75,8 @@ class GraphMsfRos2 : public GraphMsfClassic, public GraphMsfHolistic { // Services bool srvOfflineSmootherOptimizeCallback( - const std::shared_ptr req, - std::shared_ptr res); + const std::shared_ptr req, + std::shared_ptr res); // Publishing ----------------------------------- virtual void publishState( @@ -163,7 +162,7 @@ class GraphMsfRos2 : public GraphMsfClassic, public GraphMsfHolistic { geometry_msgs::msg::Vector3Stamped::SharedPtr gyroBiasMsgPtr_; // Services - rclcpp::Service::SharedPtr srvSmootherOptimize_; + rclcpp::Service::SharedPtr srvSmootherOptimize_; // Last Optimized State Timestamp double lastOptimizedStateTimestamp_ = 0.0; diff --git a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp index 923a1944..007a32f4 100644 --- a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp +++ b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp @@ -4,6 +4,7 @@ // ROS2 #include #include +#include // Workspace #include "graph_msf_ros2/constants.h" @@ -210,19 +211,19 @@ void GraphMsfRos2::initializeServices(rclcpp::Node& node) { RCLCPP_INFO(node.get_logger(), "Initializing services."); // Trigger offline smoother optimization - srvSmootherOptimize_ = node.create_service( + srvSmootherOptimize_ = node.create_service( "/graph_msf/trigger_offline_optimization", std::bind(&GraphMsfRos2::srvOfflineSmootherOptimizeCallback, this, std::placeholders::_1, std::placeholders::_2)); } bool GraphMsfRos2::srvOfflineSmootherOptimizeCallback( - const std::shared_ptr req, - std::shared_ptr res) { - // Max Iterations from service call - int maxIterations = req->max_optimization_iterations; + const std::shared_ptr req, + std::shared_ptr res) { + // Hardcode max iterations (you can make this configurable via parameter if needed) + int maxIterations = 100; // Trigger offline smoother optimization and create response - if (GraphMsf::optimizeSlowBatchSmoother(maxIterations, optimizationResultLoggingPath, false)) { + if (optimizeSlowBatchSmoother(maxIterations, optimizationResultLoggingPath, false)) { res->success = true; res->message = "Optimization successful."; } else { diff --git a/ros2/graph_msf_ros2_msgs/CMakeLists.txt b/ros2/graph_msf_ros2_msgs/CMakeLists.txt index db7157fb..d7645a4c 100644 --- a/ros2/graph_msf_ros2_msgs/CMakeLists.txt +++ b/ros2/graph_msf_ros2_msgs/CMakeLists.txt @@ -15,6 +15,79 @@ rosidl_generate_interfaces(${PROJECT_NAME} DEPENDENCIES nav_msgs std_msgs ) -# Package export and installation +########### +## Build ## +########### + +# # Create an INTERFACE library for the headers +# add_library(${PROJECT_NAME}_INTERFACE INTERFACE) +# # Exported alias target +# add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME}_INTERFACE) + +# # Include directories for the library +# target_include_directories(${PROJECT_NAME}_INTERFACE +# INTERFACE +# $ +# $ +# ) + +# # Link dependencies (for plain CMake usage) +# target_link_libraries(${PROJECT_NAME}_INTERFACE +# INTERFACE +# nav_msgs::nav_msgs__rosidl_generator_cpp +# std_msgs::std_msgs__rosidl_generator_cpp +# rosidl_typesupport_cpp +# ) + +############# +## Install ## +############# + +# # Export the include directories and linked libraries +# install( +# TARGETS ${PROJECT_NAME}_INTERFACE +# EXPORT ${PROJECT_NAME}Targets # Export the target for downstream usage +# ARCHIVE DESTINATION lib +# LIBRARY DESTINATION lib +# RUNTIME DESTINATION bin +# ) + +# Install the include directory +install(DIRECTORY include/ + DESTINATION include +) + +# # Install the export file for downstream projects +# install(EXPORT ${PROJECT_NAME}Targets +# FILE ${PROJECT_NAME}Targets.cmake +# NAMESPACE ${PROJECT_NAME}:: +# DESTINATION share/${PROJECT_NAME}/cmake +# ) + +# # Version +# include(CMakePackageConfigHelpers) +# write_basic_package_version_file( +# "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" +# VERSION 1.0.0 +# COMPATIBILITY SameMajorVersion +# ) + +# # Configure +# configure_package_config_file( +# "${PROJECT_NAME}Config.cmake.in" +# "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" +# INSTALL_DESTINATION share/${PROJECT_NAME}/cmake +# ) + +# # Files +# install(FILES +# "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}Config.cmake" +# "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake" +# DESTINATION share/${PROJECT_NAME}/cmake +# ) + +# # Package export and installation +# ament_export_include_directories(include) +# ament_export_targets(${PROJECT_NAME}Targets) ament_export_dependencies(rosidl_default_runtime nav_msgs std_msgs) ament_package() diff --git a/ros2/graph_msf_ros2_msgs/graph_msf_ros2_msgsConfig.cmake.in b/ros2/graph_msf_ros2_msgs/graph_msf_ros2_msgsConfig.cmake.in new file mode 100644 index 00000000..1902a6ed --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/graph_msf_ros2_msgsConfig.cmake.in @@ -0,0 +1,12 @@ +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) + +# Find dependencies +find_dependency(nav_msgs) +find_dependency(std_msgs) + +# Include the targets file +include("${CMAKE_CURRENT_LIST_DIR}/graph_msf_ros2_msgsTargets.cmake") + +check_required_components(graph_msf_ros2_msgs) diff --git a/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/msg/rosidl_generator_cpp__visibility_control.hpp b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/msg/rosidl_generator_cpp__visibility_control.hpp new file mode 100644 index 00000000..60d1ba86 --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/msg/rosidl_generator_cpp__visibility_control.hpp @@ -0,0 +1,42 @@ +// generated from rosidl_generator_cpp/resource/rosidl_generator_cpp__visibility_control.hpp.in +// generated code does not contain a copyright notice + +#ifndef GRAPH_MSF_ROS2_MSGS__MSG__ROSIDL_GENERATOR_CPP__VISIBILITY_CONTROL_HPP_ +#define GRAPH_MSF_ROS2_MSGS__MSG__ROSIDL_GENERATOR_CPP__VISIBILITY_CONTROL_HPP_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define ROSIDL_GENERATOR_CPP_EXPORT_graph_msf_ros2_msgs __attribute__ ((dllexport)) + #define ROSIDL_GENERATOR_CPP_IMPORT_graph_msf_ros2_msgs __attribute__ ((dllimport)) + #else + #define ROSIDL_GENERATOR_CPP_EXPORT_graph_msf_ros2_msgs __declspec(dllexport) + #define ROSIDL_GENERATOR_CPP_IMPORT_graph_msf_ros2_msgs __declspec(dllimport) + #endif + #ifdef ROSIDL_GENERATOR_CPP_BUILDING_DLL_graph_msf_ros2_msgs + #define ROSIDL_GENERATOR_CPP_PUBLIC_graph_msf_ros2_msgs ROSIDL_GENERATOR_CPP_EXPORT_graph_msf_ros2_msgs + #else + #define ROSIDL_GENERATOR_CPP_PUBLIC_graph_msf_ros2_msgs ROSIDL_GENERATOR_CPP_IMPORT_graph_msf_ros2_msgs + #endif +#else + #define ROSIDL_GENERATOR_CPP_EXPORT_graph_msf_ros2_msgs __attribute__ ((visibility("default"))) + #define ROSIDL_GENERATOR_CPP_IMPORT_graph_msf_ros2_msgs + #if __GNUC__ >= 4 + #define ROSIDL_GENERATOR_CPP_PUBLIC_graph_msf_ros2_msgs __attribute__ ((visibility("default"))) + #else + #define ROSIDL_GENERATOR_CPP_PUBLIC_graph_msf_ros2_msgs + #endif +#endif + +#ifdef __cplusplus +} +#endif + +#endif // GRAPH_MSF_ROS2_MSGS__MSG__ROSIDL_GENERATOR_CPP__VISIBILITY_CONTROL_HPP_ diff --git a/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__builder.hpp b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__builder.hpp new file mode 100644 index 00000000..1ff8aafb --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__builder.hpp @@ -0,0 +1,114 @@ +// generated from rosidl_generator_cpp/resource/idl__builder.hpp.em +// with input from graph_msf_ros2_msgs:srv/OfflineOptimizationTrigger.idl +// generated code does not contain a copyright notice + +#ifndef GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__BUILDER_HPP_ +#define GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__BUILDER_HPP_ + +#include +#include + +#include "graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__struct.hpp" +#include "rosidl_runtime_cpp/message_initialization.hpp" + + +namespace graph_msf_ros2_msgs +{ + +namespace srv +{ + +namespace builder +{ + +class Init_OfflineOptimizationTrigger_Request_max_optimization_iterations +{ +public: + Init_OfflineOptimizationTrigger_Request_max_optimization_iterations() + : msg_(::rosidl_runtime_cpp::MessageInitialization::SKIP) + {} + ::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request max_optimization_iterations(::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request::_max_optimization_iterations_type arg) + { + msg_.max_optimization_iterations = std::move(arg); + return std::move(msg_); + } + +private: + ::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request msg_; +}; + +} // namespace builder + +} // namespace srv + +template +auto build(); + +template<> +inline +auto build<::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request>() +{ + return graph_msf_ros2_msgs::srv::builder::Init_OfflineOptimizationTrigger_Request_max_optimization_iterations(); +} + +} // namespace graph_msf_ros2_msgs + + +namespace graph_msf_ros2_msgs +{ + +namespace srv +{ + +namespace builder +{ + +class Init_OfflineOptimizationTrigger_Response_message +{ +public: + explicit Init_OfflineOptimizationTrigger_Response_message(::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response & msg) + : msg_(msg) + {} + ::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response message(::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response::_message_type arg) + { + msg_.message = std::move(arg); + return std::move(msg_); + } + +private: + ::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response msg_; +}; + +class Init_OfflineOptimizationTrigger_Response_success +{ +public: + Init_OfflineOptimizationTrigger_Response_success() + : msg_(::rosidl_runtime_cpp::MessageInitialization::SKIP) + {} + Init_OfflineOptimizationTrigger_Response_message success(::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response::_success_type arg) + { + msg_.success = std::move(arg); + return Init_OfflineOptimizationTrigger_Response_message(msg_); + } + +private: + ::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response msg_; +}; + +} // namespace builder + +} // namespace srv + +template +auto build(); + +template<> +inline +auto build<::graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response>() +{ + return graph_msf_ros2_msgs::srv::builder::Init_OfflineOptimizationTrigger_Response_success(); +} + +} // namespace graph_msf_ros2_msgs + +#endif // GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__BUILDER_HPP_ diff --git a/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__struct.hpp b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__struct.hpp new file mode 100644 index 00000000..88fae57e --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__struct.hpp @@ -0,0 +1,275 @@ +// generated from rosidl_generator_cpp/resource/idl__struct.hpp.em +// with input from graph_msf_ros2_msgs:srv/OfflineOptimizationTrigger.idl +// generated code does not contain a copyright notice + +#ifndef GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__STRUCT_HPP_ +#define GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__STRUCT_HPP_ + +#include +#include +#include +#include +#include + +#include "rosidl_runtime_cpp/bounded_vector.hpp" +#include "rosidl_runtime_cpp/message_initialization.hpp" + + +#ifndef _WIN32 +# define DEPRECATED__graph_msf_ros2_msgs__srv__OfflineOptimizationTrigger_Request __attribute__((deprecated)) +#else +# define DEPRECATED__graph_msf_ros2_msgs__srv__OfflineOptimizationTrigger_Request __declspec(deprecated) +#endif + +namespace graph_msf_ros2_msgs +{ + +namespace srv +{ + +// message struct +template +struct OfflineOptimizationTrigger_Request_ +{ + using Type = OfflineOptimizationTrigger_Request_; + + explicit OfflineOptimizationTrigger_Request_(rosidl_runtime_cpp::MessageInitialization _init = rosidl_runtime_cpp::MessageInitialization::ALL) + { + if (rosidl_runtime_cpp::MessageInitialization::ALL == _init || + rosidl_runtime_cpp::MessageInitialization::ZERO == _init) + { + this->max_optimization_iterations = 0ll; + } + } + + explicit OfflineOptimizationTrigger_Request_(const ContainerAllocator & _alloc, rosidl_runtime_cpp::MessageInitialization _init = rosidl_runtime_cpp::MessageInitialization::ALL) + { + (void)_alloc; + if (rosidl_runtime_cpp::MessageInitialization::ALL == _init || + rosidl_runtime_cpp::MessageInitialization::ZERO == _init) + { + this->max_optimization_iterations = 0ll; + } + } + + // field types and members + using _max_optimization_iterations_type = + int64_t; + _max_optimization_iterations_type max_optimization_iterations; + + // setters for named parameter idiom + Type & set__max_optimization_iterations( + const int64_t & _arg) + { + this->max_optimization_iterations = _arg; + return *this; + } + + // constant declarations + + // pointer types + using RawPtr = + graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request_ *; + using ConstRawPtr = + const graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request_ *; + using SharedPtr = + std::shared_ptr>; + using ConstSharedPtr = + std::shared_ptr const>; + + template>> + using UniquePtrWithDeleter = + std::unique_ptr, Deleter>; + + using UniquePtr = UniquePtrWithDeleter<>; + + template>> + using ConstUniquePtrWithDeleter = + std::unique_ptr const, Deleter>; + using ConstUniquePtr = ConstUniquePtrWithDeleter<>; + + using WeakPtr = + std::weak_ptr>; + using ConstWeakPtr = + std::weak_ptr const>; + + // pointer types similar to ROS 1, use SharedPtr / ConstSharedPtr instead + // NOTE: Can't use 'using' here because GNU C++ can't parse attributes properly + typedef DEPRECATED__graph_msf_ros2_msgs__srv__OfflineOptimizationTrigger_Request + std::shared_ptr> + Ptr; + typedef DEPRECATED__graph_msf_ros2_msgs__srv__OfflineOptimizationTrigger_Request + std::shared_ptr const> + ConstPtr; + + // comparison operators + bool operator==(const OfflineOptimizationTrigger_Request_ & other) const + { + if (this->max_optimization_iterations != other.max_optimization_iterations) { + return false; + } + return true; + } + bool operator!=(const OfflineOptimizationTrigger_Request_ & other) const + { + return !this->operator==(other); + } +}; // struct OfflineOptimizationTrigger_Request_ + +// alias to use template instance with default allocator +using OfflineOptimizationTrigger_Request = + graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request_>; + +// constant definitions + +} // namespace srv + +} // namespace graph_msf_ros2_msgs + + +#ifndef _WIN32 +# define DEPRECATED__graph_msf_ros2_msgs__srv__OfflineOptimizationTrigger_Response __attribute__((deprecated)) +#else +# define DEPRECATED__graph_msf_ros2_msgs__srv__OfflineOptimizationTrigger_Response __declspec(deprecated) +#endif + +namespace graph_msf_ros2_msgs +{ + +namespace srv +{ + +// message struct +template +struct OfflineOptimizationTrigger_Response_ +{ + using Type = OfflineOptimizationTrigger_Response_; + + explicit OfflineOptimizationTrigger_Response_(rosidl_runtime_cpp::MessageInitialization _init = rosidl_runtime_cpp::MessageInitialization::ALL) + { + if (rosidl_runtime_cpp::MessageInitialization::ALL == _init || + rosidl_runtime_cpp::MessageInitialization::ZERO == _init) + { + this->success = false; + this->message = ""; + } + } + + explicit OfflineOptimizationTrigger_Response_(const ContainerAllocator & _alloc, rosidl_runtime_cpp::MessageInitialization _init = rosidl_runtime_cpp::MessageInitialization::ALL) + : message(_alloc) + { + if (rosidl_runtime_cpp::MessageInitialization::ALL == _init || + rosidl_runtime_cpp::MessageInitialization::ZERO == _init) + { + this->success = false; + this->message = ""; + } + } + + // field types and members + using _success_type = + bool; + _success_type success; + using _message_type = + std::basic_string, typename std::allocator_traits::template rebind_alloc>; + _message_type message; + + // setters for named parameter idiom + Type & set__success( + const bool & _arg) + { + this->success = _arg; + return *this; + } + Type & set__message( + const std::basic_string, typename std::allocator_traits::template rebind_alloc> & _arg) + { + this->message = _arg; + return *this; + } + + // constant declarations + + // pointer types + using RawPtr = + graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response_ *; + using ConstRawPtr = + const graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response_ *; + using SharedPtr = + std::shared_ptr>; + using ConstSharedPtr = + std::shared_ptr const>; + + template>> + using UniquePtrWithDeleter = + std::unique_ptr, Deleter>; + + using UniquePtr = UniquePtrWithDeleter<>; + + template>> + using ConstUniquePtrWithDeleter = + std::unique_ptr const, Deleter>; + using ConstUniquePtr = ConstUniquePtrWithDeleter<>; + + using WeakPtr = + std::weak_ptr>; + using ConstWeakPtr = + std::weak_ptr const>; + + // pointer types similar to ROS 1, use SharedPtr / ConstSharedPtr instead + // NOTE: Can't use 'using' here because GNU C++ can't parse attributes properly + typedef DEPRECATED__graph_msf_ros2_msgs__srv__OfflineOptimizationTrigger_Response + std::shared_ptr> + Ptr; + typedef DEPRECATED__graph_msf_ros2_msgs__srv__OfflineOptimizationTrigger_Response + std::shared_ptr const> + ConstPtr; + + // comparison operators + bool operator==(const OfflineOptimizationTrigger_Response_ & other) const + { + if (this->success != other.success) { + return false; + } + if (this->message != other.message) { + return false; + } + return true; + } + bool operator!=(const OfflineOptimizationTrigger_Response_ & other) const + { + return !this->operator==(other); + } +}; // struct OfflineOptimizationTrigger_Response_ + +// alias to use template instance with default allocator +using OfflineOptimizationTrigger_Response = + graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response_>; + +// constant definitions + +} // namespace srv + +} // namespace graph_msf_ros2_msgs + +namespace graph_msf_ros2_msgs +{ + +namespace srv +{ + +struct OfflineOptimizationTrigger +{ + using Request = graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request; + using Response = graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response; +}; + +} // namespace srv + +} // namespace graph_msf_ros2_msgs + +#endif // GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__STRUCT_HPP_ diff --git a/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__traits.hpp b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__traits.hpp new file mode 100644 index 00000000..74d9c1f8 --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__traits.hpp @@ -0,0 +1,273 @@ +// generated from rosidl_generator_cpp/resource/idl__traits.hpp.em +// with input from graph_msf_ros2_msgs:srv/OfflineOptimizationTrigger.idl +// generated code does not contain a copyright notice + +#ifndef GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__TRAITS_HPP_ +#define GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__TRAITS_HPP_ + +#include + +#include +#include +#include + +#include "graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__struct.hpp" +#include "rosidl_runtime_cpp/traits.hpp" + +namespace graph_msf_ros2_msgs +{ + +namespace srv +{ + +inline void to_flow_style_yaml( + const OfflineOptimizationTrigger_Request & msg, + std::ostream & out) +{ + out << "{"; + // member: max_optimization_iterations + { + out << "max_optimization_iterations: "; + rosidl_generator_traits::value_to_yaml(msg.max_optimization_iterations, out); + } + out << "}"; +} // NOLINT(readability/fn_size) + +inline void to_block_style_yaml( + const OfflineOptimizationTrigger_Request & msg, + std::ostream & out, size_t indentation = 0) +{ + // member: max_optimization_iterations + { + if (indentation > 0) { + out << std::string(indentation, ' '); + } + out << "max_optimization_iterations: "; + rosidl_generator_traits::value_to_yaml(msg.max_optimization_iterations, out); + out << "\n"; + } +} // NOLINT(readability/fn_size) + +inline std::string to_yaml(const OfflineOptimizationTrigger_Request & msg, bool use_flow_style = false) +{ + std::ostringstream out; + if (use_flow_style) { + to_flow_style_yaml(msg, out); + } else { + to_block_style_yaml(msg, out); + } + return out.str(); +} + +} // namespace srv + +} // namespace graph_msf_ros2_msgs + +namespace rosidl_generator_traits +{ + +[[deprecated("use graph_msf_ros2_msgs::srv::to_block_style_yaml() instead")]] +inline void to_yaml( + const graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request & msg, + std::ostream & out, size_t indentation = 0) +{ + graph_msf_ros2_msgs::srv::to_block_style_yaml(msg, out, indentation); +} + +[[deprecated("use graph_msf_ros2_msgs::srv::to_yaml() instead")]] +inline std::string to_yaml(const graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request & msg) +{ + return graph_msf_ros2_msgs::srv::to_yaml(msg); +} + +template<> +inline const char * data_type() +{ + return "graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Request"; +} + +template<> +inline const char * name() +{ + return "graph_msf_ros2_msgs/srv/OfflineOptimizationTrigger_Request"; +} + +template<> +struct has_fixed_size + : std::integral_constant {}; + +template<> +struct has_bounded_size + : std::integral_constant {}; + +template<> +struct is_message + : std::true_type {}; + +} // namespace rosidl_generator_traits + +namespace graph_msf_ros2_msgs +{ + +namespace srv +{ + +inline void to_flow_style_yaml( + const OfflineOptimizationTrigger_Response & msg, + std::ostream & out) +{ + out << "{"; + // member: success + { + out << "success: "; + rosidl_generator_traits::value_to_yaml(msg.success, out); + out << ", "; + } + + // member: message + { + out << "message: "; + rosidl_generator_traits::value_to_yaml(msg.message, out); + } + out << "}"; +} // NOLINT(readability/fn_size) + +inline void to_block_style_yaml( + const OfflineOptimizationTrigger_Response & msg, + std::ostream & out, size_t indentation = 0) +{ + // member: success + { + if (indentation > 0) { + out << std::string(indentation, ' '); + } + out << "success: "; + rosidl_generator_traits::value_to_yaml(msg.success, out); + out << "\n"; + } + + // member: message + { + if (indentation > 0) { + out << std::string(indentation, ' '); + } + out << "message: "; + rosidl_generator_traits::value_to_yaml(msg.message, out); + out << "\n"; + } +} // NOLINT(readability/fn_size) + +inline std::string to_yaml(const OfflineOptimizationTrigger_Response & msg, bool use_flow_style = false) +{ + std::ostringstream out; + if (use_flow_style) { + to_flow_style_yaml(msg, out); + } else { + to_block_style_yaml(msg, out); + } + return out.str(); +} + +} // namespace srv + +} // namespace graph_msf_ros2_msgs + +namespace rosidl_generator_traits +{ + +[[deprecated("use graph_msf_ros2_msgs::srv::to_block_style_yaml() instead")]] +inline void to_yaml( + const graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response & msg, + std::ostream & out, size_t indentation = 0) +{ + graph_msf_ros2_msgs::srv::to_block_style_yaml(msg, out, indentation); +} + +[[deprecated("use graph_msf_ros2_msgs::srv::to_yaml() instead")]] +inline std::string to_yaml(const graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response & msg) +{ + return graph_msf_ros2_msgs::srv::to_yaml(msg); +} + +template<> +inline const char * data_type() +{ + return "graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger_Response"; +} + +template<> +inline const char * name() +{ + return "graph_msf_ros2_msgs/srv/OfflineOptimizationTrigger_Response"; +} + +template<> +struct has_fixed_size + : std::integral_constant {}; + +template<> +struct has_bounded_size + : std::integral_constant {}; + +template<> +struct is_message + : std::true_type {}; + +} // namespace rosidl_generator_traits + +namespace rosidl_generator_traits +{ + +template<> +inline const char * data_type() +{ + return "graph_msf_ros2_msgs::srv::OfflineOptimizationTrigger"; +} + +template<> +inline const char * name() +{ + return "graph_msf_ros2_msgs/srv/OfflineOptimizationTrigger"; +} + +template<> +struct has_fixed_size + : std::integral_constant< + bool, + has_fixed_size::value && + has_fixed_size::value + > +{ +}; + +template<> +struct has_bounded_size + : std::integral_constant< + bool, + has_bounded_size::value && + has_bounded_size::value + > +{ +}; + +template<> +struct is_service + : std::true_type +{ +}; + +template<> +struct is_service_request + : std::true_type +{ +}; + +template<> +struct is_service_response + : std::true_type +{ +}; + +} // namespace rosidl_generator_traits + +#endif // GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__TRAITS_HPP_ diff --git a/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__type_support.hpp b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__type_support.hpp new file mode 100644 index 00000000..40008f34 --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__type_support.hpp @@ -0,0 +1,71 @@ +// generated from rosidl_generator_cpp/resource/idl__type_support.hpp.em +// with input from graph_msf_ros2_msgs:srv/OfflineOptimizationTrigger.idl +// generated code does not contain a copyright notice + +#ifndef GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__TYPE_SUPPORT_HPP_ +#define GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__TYPE_SUPPORT_HPP_ + +#include "rosidl_typesupport_interface/macros.h" + +#include "graph_msf_ros2_msgs/msg/rosidl_generator_cpp__visibility_control.hpp" + +#include "rosidl_typesupport_cpp/service_type_support.hpp" + +#ifdef __cplusplus +extern "C" +{ +#endif +// Forward declare the get type support functions for this type. +ROSIDL_GENERATOR_CPP_PUBLIC_graph_msf_ros2_msgs +const rosidl_service_type_support_t * + ROSIDL_TYPESUPPORT_INTERFACE__SERVICE_SYMBOL_NAME( + rosidl_typesupport_cpp, + graph_msf_ros2_msgs, + srv, + OfflineOptimizationTrigger +)(); +#ifdef __cplusplus +} +#endif + +#include "rosidl_typesupport_cpp/message_type_support.hpp" + +#ifdef __cplusplus +extern "C" +{ +#endif +// Forward declare the get type support functions for this type. +ROSIDL_GENERATOR_CPP_PUBLIC_graph_msf_ros2_msgs +const rosidl_message_type_support_t * + ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME( + rosidl_typesupport_cpp, + graph_msf_ros2_msgs, + srv, + OfflineOptimizationTrigger_Request +)(); +#ifdef __cplusplus +} +#endif + +// already included above +// #include "rosidl_typesupport_cpp/message_type_support.hpp" + +#ifdef __cplusplus +extern "C" +{ +#endif +// Forward declare the get type support functions for this type. +ROSIDL_GENERATOR_CPP_PUBLIC_graph_msf_ros2_msgs +const rosidl_message_type_support_t * + ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME( + rosidl_typesupport_cpp, + graph_msf_ros2_msgs, + srv, + OfflineOptimizationTrigger_Response +)(); +#ifdef __cplusplus +} +#endif + + +#endif // GRAPH_MSF_ROS2_MSGS__SRV__DETAIL__OFFLINE_OPTIMIZATION_TRIGGER__TYPE_SUPPORT_HPP_ diff --git a/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/offline_optimization_trigger.hpp b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/offline_optimization_trigger.hpp new file mode 100644 index 00000000..5d71d40a --- /dev/null +++ b/ros2/graph_msf_ros2_msgs/include/graph_msf_ros2_msgs/srv/offline_optimization_trigger.hpp @@ -0,0 +1,12 @@ +// generated from rosidl_generator_cpp/resource/idl.hpp.em +// generated code does not contain a copyright notice + +#ifndef GRAPH_MSF_ROS2_MSGS__SRV__OFFLINE_OPTIMIZATION_TRIGGER_HPP_ +#define GRAPH_MSF_ROS2_MSGS__SRV__OFFLINE_OPTIMIZATION_TRIGGER_HPP_ + +#include "graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__struct.hpp" +#include "graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__builder.hpp" +#include "graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__traits.hpp" +#include "graph_msf_ros2_msgs/srv/detail/offline_optimization_trigger__type_support.hpp" + +#endif // GRAPH_MSF_ROS2_MSGS__SRV__OFFLINE_OPTIMIZATION_TRIGGER_HPP_ From fda9612a957fce5b7f9684e76026b6e0509b25a0 Mon Sep 17 00:00:00 2001 From: Julian Date: Fri, 25 Jul 2025 12:25:53 +0200 Subject: [PATCH 58/60] updated --- ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp index 007a32f4..3577f968 100644 --- a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp +++ b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp @@ -216,10 +216,9 @@ void GraphMsfRos2::initializeServices(rclcpp::Node& node) { std::bind(&GraphMsfRos2::srvOfflineSmootherOptimizeCallback, this, std::placeholders::_1, std::placeholders::_2)); } -bool GraphMsfRos2::srvOfflineSmootherOptimizeCallback( - const std::shared_ptr req, - std::shared_ptr res) { - // Hardcode max iterations (you can make this configurable via parameter if needed) +bool GraphMsfRos2::srvOfflineSmootherOptimizeCallback(const std::shared_ptr req, + std::shared_ptr res) { + // Hardcode max iterations (you can make this configurable via parameter if needed) int maxIterations = 100; // Trigger offline smoother optimization and create response @@ -428,6 +427,9 @@ void GraphMsfRos2::publishState( graph_msf::GraphMsfRos2::extractCovariancesFromOptimizedState(poseCovarianceRos, twistCovarianceRos, optimizedStateWithCovarianceAndBiasPtr); + // Odometry messages + publishImuOdoms(integratedNavStatePtr, poseCovarianceRos, twistCovarianceRos); + // Variances (only diagonal elements) Eigen::Vector3d positionVarianceRos = poseCovarianceRos.block<3, 3>(0, 0).diagonal(); Eigen::Vector3d orientationVarianceRos = poseCovarianceRos.block<3, 3>(3, 3).diagonal(); @@ -451,9 +453,6 @@ void GraphMsfRos2::publishNonTimeCriticalData( // Time const double& timeK = integratedNavStatePtr->getTimeK(); // Alias - // Odometry messages - publishImuOdoms(integratedNavStatePtr, poseCovarianceRos, twistCovarianceRos); - // Publish to TF // B_O Eigen::Isometry3d T_B_Ok = @@ -605,7 +604,7 @@ void GraphMsfRos2::publishOptimizedStateAndBias( T_sensor_sensorCorrected); } } - } // Publishing of Transforms + } // Publishing of Transforms } // Lower Level Functions From cc2f77baab8b9052cea1a67f1a4cb9f1d63a1c40 Mon Sep 17 00:00:00 2001 From: Julian Date: Thu, 31 Jul 2025 18:52:46 +0200 Subject: [PATCH 59/60] faster update rate of TF transforms for better visualization in rviz --- ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp | 47 ++++++++++---------- ros2/graph_msf_ros2/src/lib/readParams.cpp | 3 -- 2 files changed, 23 insertions(+), 27 deletions(-) diff --git a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp index 3577f968..ee091d13 100644 --- a/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp +++ b/ros2/graph_msf_ros2/src/lib/GraphMsfRos2.cpp @@ -479,7 +479,7 @@ void GraphMsfRos2::publishNonTimeCriticalData( void GraphMsfRos2::publishOptimizedStateAndBias( const std::shared_ptr optimizedStateWithCovarianceAndBiasPtr, const Eigen::Matrix& poseCovarianceRos, const Eigen::Matrix& twistCovarianceRos, const double timeStamp) { - // A: Publish Odometry and IMU Biases + // A: Publish Odometry and IMU Biases at update rate if (optimizedStateWithCovarianceAndBiasPtr != nullptr && optimizedStateWithCovarianceAndBiasPtr->getTimeK() - lastOptimizedStateTimestamp_ > 1e-03) { // Time of this optimized state @@ -527,7 +527,7 @@ void GraphMsfRos2::publishOptimizedStateAndBias( } } // Publishing odometry and biases - // B: Publish Transforms + // B: Publish Transforms at imu rate if (optimizedStateWithCovarianceAndBiasPtr != nullptr && timeStamp - lastIntegratedStateTimestamp_ > 1e-03) { lastIntegratedStateTimestamp_ = timeStamp; // TFs in Optimized State @@ -550,30 +550,29 @@ void GraphMsfRos2::publishOptimizedStateAndBias( else { T_W_M = framePairTransformMapIterator.second; mapFrameName = framePairTransformMapIterator.first.second; + } - // B. Publish TransformStamped for Aligned Frames - std::string transformTopic = "/graph_msf/transform_" + worldFrameName + "_to_" + mapFrameName + referenceFrameAlignedNameId; - auto poseWithCovarianceStampedMsgPtr = std::make_shared(); - addToPoseWithCovarianceStampedMsg( - poseWithCovarianceStampedMsgPtr, staticTransformsPtr_->getWorldFrame(), - rclcpp::Time(optimizedStateWithCovarianceAndBiasPtr->getTimeK() * 1e9), T_W_M, - optimizedStateWithCovarianceAndBiasPtr->getReferenceFrameTransformsCovariance().rv_T_frame1_frame2( - framePairTransformMapIterator.first.first, framePairTransformMapIterator.first.second)); - // Check whether publisher already exists - if (pubPoseStampedByTopicMap_.find(transformTopic) == pubPoseStampedByTopicMap_.end()) { - pubPoseStampedByTopicMap_[transformTopic] = - node_->create_publisher(transformTopic, 1); - RCLCPP_INFO(node_->get_logger(), "Initialized publisher for %s", transformTopic.c_str()); - } - if (pubPoseStampedByTopicMap_[transformTopic]->get_subscription_count() > 0) { - pubPoseStampedByTopicMap_[transformTopic]->publish(*poseWithCovarianceStampedMsgPtr); - } + // B. Publish TransformStamped for Aligned Frames + std::string transformTopic = "/graph_msf/transform_" + worldFrameName + "_to_" + mapFrameName + referenceFrameAlignedNameId; + auto poseWithCovarianceStampedMsgPtr = std::make_shared(); + addToPoseWithCovarianceStampedMsg( + poseWithCovarianceStampedMsgPtr, staticTransformsPtr_->getWorldFrame(), + rclcpp::Time(timeStamp * 1e9), T_W_M, + optimizedStateWithCovarianceAndBiasPtr->getReferenceFrameTransformsCovariance().rv_T_frame1_frame2( + framePairTransformMapIterator.first.first, framePairTransformMapIterator.first.second)); + // Check whether publisher already exists + if (pubPoseStampedByTopicMap_.find(transformTopic) == pubPoseStampedByTopicMap_.end()) { + pubPoseStampedByTopicMap_[transformTopic] = + node_->create_publisher(transformTopic, 1); + RCLCPP_INFO(node_->get_logger(), "Initialized publisher for %s", transformTopic.c_str()); + } + if (pubPoseStampedByTopicMap_[transformTopic]->get_subscription_count() > 0) { + pubPoseStampedByTopicMap_[transformTopic]->publish(*poseWithCovarianceStampedMsgPtr); } // C. Publish TF Tree publishTfTreeTransform(worldFrameName, mapFrameName + referenceFrameAlignedNameId, - optimizedStateWithCovarianceAndBiasPtr->getTimeK(), T_W_M); - + timeStamp, T_W_M); } // Case 2: Other transformations else { @@ -585,7 +584,7 @@ void GraphMsfRos2::publishOptimizedStateAndBias( std::string transformTopic = "/graph_msf/transform_" + sensorFrameName + "_to_" + sensorFrameNameCorrected; auto poseWithCovarianceStampedMsgPtr = std::make_shared(); addToPoseWithCovarianceStampedMsg( - poseWithCovarianceStampedMsgPtr, sensorFrameName, rclcpp::Time(optimizedStateWithCovarianceAndBiasPtr->getTimeK() * 1e9), + poseWithCovarianceStampedMsgPtr, sensorFrameName, rclcpp::Time(timeStamp * 1e9), T_sensor_sensorCorrected, optimizedStateWithCovarianceAndBiasPtr->getReferenceFrameTransformsCovariance().rv_T_frame1_frame2(sensorFrameName, sensorFrameNameCorrected)); @@ -600,10 +599,10 @@ void GraphMsfRos2::publishOptimizedStateAndBias( } // B. Publish TF Tree - publishTfTreeTransform(sensorFrameName, sensorFrameNameCorrected, optimizedStateWithCovarianceAndBiasPtr->getTimeK(), + publishTfTreeTransform(sensorFrameName, sensorFrameNameCorrected, timeStamp, T_sensor_sensorCorrected); } - } + } // for each frame pair transform } // Publishing of Transforms } diff --git a/ros2/graph_msf_ros2/src/lib/readParams.cpp b/ros2/graph_msf_ros2/src/lib/readParams.cpp index 9b4275aa..fc6216f3 100644 --- a/ros2/graph_msf_ros2/src/lib/readParams.cpp +++ b/ros2/graph_msf_ros2/src/lib/readParams.cpp @@ -163,9 +163,6 @@ void GraphMsfRos2::readParams() { std::filesystem::create_directories(optimizationResultLoggingPath); } - // TODO: Remove hard coded path - optimizationResultLoggingPath = "/smb_ros2_workspace/logging/"; - // Print the logging path std::cout << "-------------------------------------------------" << std::endl; RCLCPP_INFO(node_->get_logger(), "Optimization results will be logged to: %s", From ddca56d54a77cbf9c42af7b224a556628c164bb3 Mon Sep 17 00:00:00 2001 From: Julian Date: Sat, 2 Aug 2025 17:35:15 +0200 Subject: [PATCH 60/60] remove double export of cmake and ament that can create issues under certain circumstances --- ros2/graph_msf_ros2/CMakeLists.txt | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/ros2/graph_msf_ros2/CMakeLists.txt b/ros2/graph_msf_ros2/CMakeLists.txt index a69811f2..9338b635 100644 --- a/ros2/graph_msf_ros2/CMakeLists.txt +++ b/ros2/graph_msf_ros2/CMakeLists.txt @@ -143,21 +143,4 @@ install(FILES ) # Export ament package -ament_export_include_directories(include) -ament_export_libraries(${PROJECT_NAME}) -ament_export_targets(${PROJECT_NAME}Targets) -ament_export_dependencies( - geometry_msgs - nav_msgs - sensor_msgs - std_msgs - tf2 - tf2_ros - tf2_eigen - std_srvs - visualization_msgs - rosidl_default_runtime - graph_msf_ros2_msgs -) - ament_package()