I fount this error

[cisrobot]

[online_node-1] [/home/marin/marine/build/rko_lio/_deps/sophus-src/sophus/so3.hpp:747] SOPHUS_ENSURE failed:
[online_node-1] SO3::exp failed! omega: nan nan nan, real: nan, img: nan
[ERROR] [online_node-1]: process has died [pid 22875, exit code -6, cmd
'/home/marin/marine/install/rko_lio/lib/rko_lio/online_node --ros-args --log-level info --ros-args --params-file /tmp/launch_params_kzqrman1'].

I'm using velodyne vlp-16 and PX4 GPS. Also, I have to do project to ROS2 Humble. And topic name is /mavros/imu/data(it's px4 gps imu) and /velodyne_points.

but, when I run my robot, I saw this error.

And, this is my launch file setting. What can I do?

from pathlib import Path

import launch_ros.actions
import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.substitutions import LaunchConfiguration

#import logging

logging.root.setLevel(logging.DEBUG)

offline_only_parameters = [
{
"name": "bag_path",
"default": "/home/marin/marine/src/rko_lio/bag",
"description": "[Offline node only] ROS bag path to process (required)",
"required": True,
},
{
"name": "skip_to_time",
"default": "0.0",
"type": "float",
"description": "[Offline node only] Skip to timestamp in the bag (seconds)",
},
]

configurable_parameters = [
{
"name": "imu_topic",
"default": "/mavros/imu/data",
"description": "IMU input topic (required)",
"required": True,
},
{
"name": "lidar_topic",
"default": "/velodyne_points",
"description": "LiDAR pointcloud topic (required)",
"required": True,
},
{
"name": "imu_frame",
"default": "base_link",
"description": "IMU frame, can be left empty if message frame id is to be used",
},
{
"name": "lidar_frame",
"default": "velodyne",
"description": "LiDAR frame, can be left empty if message frame id is to be used",
},
{
"name": "base_frame",
"default": "base_link",
"description": "Robot base frame, or the frame of estimation for odometry (required)",
"required": True,
},
{
"name": "odom_frame",
"default": "odom",
"description": "Odom frame id, used for publishing the tf from base frame to odom frame",
},
{
"name": "odom_topic",
"default": "/rko_lio/odometry",
"description": "Odometry topic name",
},
{
"name": "invert_odom_tf",
"default": "False",
"type": "bool",
"description": "Invert odometry transform if required so that the base frame is the parent and odom frame is the child in the TF tree",
},
{
"name": "publish_local_map",
"default": "False",
"type": "bool",
"description": "Publish local map",
},
{
"name": "map_topic",
"default": "/rko_lio/local_map",
"description": "Local map topic. Published if publish_local_map is true",
},
{
"name": "publish_map_after",
"default": "1.0",
"type": "float",
"description": "Seconds between each local map publishing",
},
{
"name": "publish_lidar_acceleration",
"default": "False",
"type": "bool",
"description": "Publish the linear acceleration of the base_frame expressed in base_frame coordinates. Note that this acceleration can be quite noisy, as it is essentially a double time derivative of the pose update from the lidar scan registration (similar to the twist/velocity in the odometry topic). The topic name is /rko_lio/lidar_acceleration.",
},
# lio parameters
{
"name": "deskew",
"default": "True", #False
"type": "bool",
"description": "Deskew the point cloud before registration, requires timestamps in the Lidar messages",
},
{
"name": "voxel_size",
"default": "1.0", #1.0
"type": "float",
"description": "Voxel size for the local map (meters) used for odometry",
},
{
"name": "max_points_per_voxel",
"default": "20", #20
"type": "int",
"description": "Max points per voxel",
},
{
"name": "max_range",
"default": "100", #100
"type": "float",
"description": "Max valid LiDAR range (meters)",
},
{
"name": "min_range",
"default": "1.0", #1,0
"type": "float",
"description": "Min valid LiDAR range (meters)",
},
{
"name": "max_correspondance_distance",
"default": "0.5",
"type": "float",
"description": "ICP correspondence distance threshold (meters)",
},
{
"name": "convergence_criterion",
"default": "0.00001",
"type": "float",
"description": "Optimization stopping condition for ICP",
},
{
"name": "max_num_threads",
"default": "0",
"type": "int",
"description": "Number of threads used for data association in ICP",
},
{
"name": "publish_deskewed_scan",
"default": "false",
"type": "bool",
"description": "Publish deskewed scan for visualization",
},
{
"name": "initialization_phase",
"default": "False", #False
"type": "bool",
"description": "Use the IMU data between the first two frames to initialize IMU bias and system orientation. Assumes the system is at rest between these two frames. WARNING: If this is enabled, but the odometry starts while the system is in motion, the odometry will likely not work as expected. But I highly recommended enabling this while ensuring the system starts from rest.",
},
{
"name": "max_iterations",
"default": "100",
"type": "int",
"description": "Max ICP iterations",
},
{
"name": "max_expected_jerk",
"default": "3.0",
"type": "float",
"description": "Max expected jerk (m/s^3)",
},
{
"name": "double_downsample",
"default": "False", #True
"type": "bool",
"description": "Double downsample the input cloud before registration. Useful for dense Lidars. Can be disabled for sparse lidars.",
},
{
"name": "min_beta",
"default": "200",
"type": "float",
"description": "Scale parameter that decides the minimum amount of orientation regularisation applied during ICP registration.",
},
# ros params
{
"name": "mode",
"default": "online",
"description": "Launch mode: 'offline' or 'online'",
},
{
"name": "config_file",
"default": "/home/marin/marine/src/rko_lio/ros/config/default.yaml",
"description": "YAML config file to load parameters from",
},
{
"name": "run_name",
"default": "rko_lio_odometry_run",
"description": "Run name, used when saving results to results_dir",
},
{
"name": "results_dir",
"default": "results",
"description": "When the odometry node exists, it dumps a trajectory file to this directory under a 'run_name' subfolder along with the LIO configuration parameters used for that run.",
},
{
"name": "rviz",
"default": "False",
"type": "bool",
"description": "Launch RViz simultaneously",
},
{
"name": "rviz_config_file",
"default": "config/default.rviz",
"description": "RViz config file path. If it's not the default value, note that it will be passed to rviz as is.",
},
{
"name": "log_level",
"default": "info",
"description": "ROS Log level [DEBUG|INFO|WARN|ERROR|FATAL]",
},
] + offline_only_parameters

def declare_configurable_parameters(parameters):
return [
DeclareLaunchArgument(
param["name"],
default_value=param["default"],
description=param.get("description", ""),
)
for param in parameters
]

def auto_cast_params(params, param_defs):
"""
Because for some reason, I cannot use a raw bool in configurable parameters dict
because of how DeclareLaunchArgument seems to work, so i have to write it as a string.
But then my node expects a bool so I need to explicitly cast it the string to bool again...
There must be a simpler way to do all this...
"""
out = {}
for p in param_defs:
name = p["name"]
v = params.get(name)
tp = p.get("type", None)
if tp == "bool":
out[name] = str(v).lower() == "true"
elif tp == "int":
out[name] = int(v)
elif tp == "float":
out[name] = float(v)
else:
out[name] = v
return out

def get_configurable_parameters(configurable_parameters, context):
return auto_cast_params(
dict(
[
(param["name"], LaunchConfiguration(param["name"]).perform(context))
for param in configurable_parameters
]
),
configurable_parameters,
)

def yaml_to_dict(path_to_yaml):
with open(path_to_yaml, "r") as f:
return yaml.safe_load(f)

def merge_and_validate_parameters(
cli_params: dict,
file_params: dict,
mode: str,
) -> dict:
"""
Merge CLI and file parameters:
- CLI overrides file values only if non-empty
- Validate required params are provided (depending on mode)
"""
merged = {}

merged.update(file_params)

# override with CLI only if non-empty
for k, v in cli_params.items():
    if v not in ("", None):
        merged[k] = v

# Validating required params
missing = []
for param in configurable_parameters:
    name = param["name"]

    # Skip offline-only params in online mode
    if mode == "online" and param in offline_only_parameters:
        continue

    if param.get("required", False):
        if name not in merged or not merged.get(name):
            missing.append(name)

if missing:
    print("\n\n" + "=" * 40)
    print("[ERROR] Missing required parameter(s):")
    print(", ".join(missing))
    print("Please provide them via CLI (param:=value) or a config file.")
    print("=" * 40 + "\n\n")
    import sys

    sys.exit(1)

return merged

def prepare_rviz_config(rviz_config_file: Path, parameters: dict) -> Path:
"""
Decide which RViz config to use.
If rviz_config_file is not the default, just return it.
Otherwise, patch the default config with base_frame.
"""
default_config = Path("config/default.rviz")

# If user provided a custom config, just return it unchanged
if rviz_config_file != default_config:
    return rviz_config_file

rviz_config_file = Path(get_package_share_directory("rko_lio")) / rviz_config_file

base_frame = parameters.get("base_frame", "")
if not base_frame:
    return rviz_config_file  # no override needed

# Load default config
with open(rviz_config_file, "r") as f:
    rviz_cfg = yaml.safe_load(f)

try:
    rviz_cfg["Visualization Manager"]["Views"]["Current"][
        "Target Frame"
    ] = base_frame
except Exception as e:
    raise RuntimeError(
        f"Could not patch RViz config with base_frame ({base_frame}): {e}"
    )

# Write to a temp file
import tempfile

tmp = tempfile.NamedTemporaryFile(mode="w", suffix=".rviz", delete=False)
yaml.safe_dump(rviz_cfg, tmp)
tmp.flush()
# since its the default config file, we also want to viz the deskewed scan and local map
parameters["publish_deskewed_scan"] = True
parameters["publish_local_map"] = True

return Path(tmp.name)

def launch_setup(context, *args, **kwargs):
mode = LaunchConfiguration("mode").perform(context).lower()

# Prepare parameters
config_file = LaunchConfiguration("config_file").perform(context)
params_from_file = {} if config_file == "" else yaml_to_dict(config_file)
cli_params = get_configurable_parameters(configurable_parameters, context=context)
final_params = merge_and_validate_parameters(
    cli_params=cli_params,
    file_params=params_from_file,
    mode=mode,
)

print("\n" + "=" * 40 + "\n")
print("Using Launch configuration:\n")
print(yaml.dump(final_params, sort_keys=False, default_flow_style=False, indent=4))
print("=" * 40 + "\n")

rviz_enabled = LaunchConfiguration("rviz").perform(context).lower() == "true"
if rviz_enabled:
    rviz_config_file = prepare_rviz_config(
        Path(LaunchConfiguration("rviz_config_file").perform(context)),
        final_params,
    )

node_executable = "online_node" if mode == "online" else "offline_node"

nodes = [
    launch_ros.actions.Node(
        package="rko_lio",
        executable=node_executable,
        parameters=[final_params],
        output="screen",
        arguments=[
            "--ros-args",
            "--log-level",
            LaunchConfiguration("log_level"),
        ],
        emulate_tty=True,
    )
]

if rviz_enabled:
    nodes.append(
        launch_ros.actions.Node(
            package="rviz2",
            executable="rviz2",
            name="rviz2",
            arguments=["-d", rviz_config_file.as_posix()],
            output="screen",
        )
    )

return nodes

def generate_launch_description():
return LaunchDescription(
declare_configurable_parameters(configurable_parameters)
+ [OpaqueFunction(function=launch_setup)]
)

[mehermvr]

Can you please tell me or ensure the following so that I can debug this better.

1. Please pull from master to be on the latest commit. If you already are, ignore this.
2. Thank you for posting the launch file, but it's also a bit hard to make out if you've made any changes to the configuration like that. Instead can I just ask you to post the console log, it should look something like this:

ros2 launch rko_lio odometry.launch.py <...any cli overrides>
...some ros log lines here, the following is important

========================================

Using Launch configuration:

imu_topic: /lidar/horizontal/imu
lidar_topic: /lidar/horizontal/points
base_frame: ac_0
odom_frame: odom
odom_topic: /rko_lio/odometry
invert_odom_tf: false
publish_local_map: false
map_topic: /rko_lio/local_map
...and please include all the other launch parameters
========================================

3. Can you also post the ros2 launch command you're using, just to see if you have any CLI overrides.

The Sophus ensure error you're getting indicates there is something wrong either with processing the data, or with the data itself. So

4. If you can share some example data (a bag), it will make it much easier for me to reproduce and debug what the issue is. You can share it either by email or just post a link here. If you email it, I won't include any details of the data in the issue itself, just the potential debug or fix i find.

[cisrobot]

1. Yes, I'm using latest version.

========================================

Using Launch configuration:

use_sim_time: false
base_frame: base_link
imu_topic: /mavros/imu/data
imu_frame: base_link
lidar_topic: /velodyne_points
lidar_frame: velodyne
odom_topic: /rko_lio/odometry
odom_frame: odom
invert_odom_tf: false
publish_local_map: false
map_topic: /rko_lio/local_map
published_deskewed_cloud: false
deskew: true
voxel_size: 1.0
max_points_per_voxel: 20
max_range: 100.0
min_range: 1.0
max_correspondance_distance: 0.5
convergence_criterion: 1.0e-05
max_num_threads: 0
initialization_phase: false
double_downsample: false
max_expected_jerk: 3.0
min_beta: 200.0
publish_map_after: 1.0
publish_lidar_acceleration: false
publish_deskewed_scan: false
max_iterations: 100
mode: online
config_file: /home/marin/marine/src/rko_lio/ros/config/default.yaml
run_name: rko_lio_odometry_run
results_dir: results
rviz: false
rviz_config_file: config/default.rviz
log_level: info
bag_path: /home/marin/marine/src/rko_lio/bag
skip_to_time: 0.0

========================================
And I changed default yaml
use_sim_time: False
base_frame: base_link
imu_topic: /mavros/imu/data
imu_frame: base_link
lidar_topic: /velodyne_points
lidar_frame: velodyne
//
extrinsic_lidar2base_quat_xyzw_xyz: [0.0, 0.0, 0.707, 0.707, 0.1, 0.2, 0.5]
// extrinsic_imu2base_quat_xyzw_xyz: [0.0, 0.0, 0.0, 1.0, -0.05, 0.0, -0.1]
odom_topic: /rko_lio/odometry
odom_frame: odom
invert_odom_tf: False
publish_local_map: odom
map_topic: /rko_lio/local_map
published_deskewed_cloud: false
lio config
deskew: True
voxel_size: 1.0
max_points_per_voxel: 20
max_range: 100.0
min_range: 1.0
max_correspondance_distance: 0.5
convergence_criterion: 0.00001
max_num_threads: 0
initialization_phase: False
double_downsample: False
max_expected_jerk: 3.0
min_beta: 200.0

Also I jest run

• ros2 launch rko_lio odometry.launch.py

[cisrobot]

Thank you for responding to my request.

[cisrobot]

can I ask one more issue? I have to use navsat_transform node. so, I will use ekf filter.
if I use ekf filter, I need to remove the part that connects odom to base_link. but, I can't find about this parameter.
how can I remove the part that connects odom -> base link?

[mehermvr]

Please pull from master again, and use the latest commit, as I fixed some other issue i noticed with the launch file in #28.

first i'll address the problem with running the odometry itself, then get to the second issue you asked.

Also I jest run
ros2 launch rko_lio odometry.launch.py

You will have to pass the config file to the launch file explicity. the file defined in ros/config/default.yaml is meant to just tell what are teh default values. I've updated this file to mention this explicitly in #28 now
So to use your config file with the launch, you will have to do

ros2 launch rko_lio odometry.launch.py config_file:=<full_or_relative_path_to>/default.yaml

You are using in your config

extrinsic_lidar2base_quat_xyzw_xyz: [0.0, 0.0, 0.707, 0.707, 0.1, 0.2, 0.5]
// extrinsic_imu2base_quat_xyzw_xyz: [0.0, 0.0, 0.0, 1.0, -0.05, 0.0, -0.1]

do you have a tf tree as well from the sensor drivers or otherwise? If your tf tree is well defined, you don't need to specify the extrinsics unless you want to override the values.
I'm not sure why you are passing one but not the other (it's commented), but please keep in mind that both extrinsics are required by the system. I do not implicitly assume one is the identity, you still have to define it as identity. This should now raise an error if you pull from master.

One more thing, you have

base_frame: base_link
imu_topic: /mavros/imu/data
imu_frame: base_link

which means imu2base should be identity. If you're relying on the tf query for extrinsic lookup, I'd check if the frame ids in the messages match the ids in the tree.

Can you please test again with the new changes I made?

[mehermvr]

can I ask one more issue? I have to use navsat_transform node. so, I will use ekf filter. if I use ekf filter, I need to remove the part that connects odom to base_link. but, I can't find about this parameter. how can I remove the part that connects odom -> base link?

Not sure I fully understand the requirement, but maybe the following will still help.
RKO LIO by default publishes a TF message from an odom_frame to the base_frame. These two frame ids are configurable. You can write a config file and change the values, for example here and here. Note again, that this default.yaml file will not be used by the odometry automatically, but you'll have to pass it explicitly as a launch argument.

In case your TF tree already has a parent frame for the base_frame, let's call this other_odom_frame, then publishing another odom_frame (parent) -> base_frame (child) transform would break the tree.
You can set invert_odom_tf to true (here, briefly mentioned here).
The TF tree will then look like this
other_odom_frame -> base_frame -> odom_frame
and your TF tree will be valid.

Currently there is no way to disable publishing the tf message. But the above should be enough to workaround this. If you need to disable the tf publish no matter what, you can comment out the code lines here until line 310