search

NeBula-Autonomy / LOCUS

Robust Lidar Odometry System
MIT License
352 stars 48 forks source link

Can not start LOCUS in any way, please help. #48

Closed Cristian-wp closed 2 years ago

Cristian-wp commented 2 years ago

Hello, I am trying to run LOCUS on my dataset. My dataset consists in a rosbag with imu (vectornav vn100), 3D lidar (VLP16), odometry topic (from a visual slam, type nav_msgs/Odometry) ad a camera image.

When I try to run locus alone and start the rosbag, I get this output:

roslaunch locus locus.launch
... logging to /var/log/extars/ros/6dabe0b2-09a6-11ed-bbec-54b203083f30/roslaunch-extars-28081.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

xacro: in-order processing became default in ROS Melodic. You can drop the option.
Using load_yaml() directly is deprecated. Use xacro.load_yaml() instead.
started roslaunch server http://extars:45129/

SUMMARY
========

PARAMETERS
 * /extars/body_filter/max_x: 0.6
 * /extars/body_filter/max_y: 0.5
 * /extars/body_filter/max_z: 1.8
 * /extars/body_filter/min_x: -1.0
 * /extars/body_filter/min_y: -0.5
 * /extars/body_filter/min_z: -1.6
 * /extars/body_filter/rotation: 0.0
 * /extars/extars/robot_description: <?xml version="1....
 * /extars/extars/tf_prefix: extars
 * /extars/locus/b_adaptive_input_voxelization: True
 * /extars/locus/b_add_keyframes_enabled: True
 * /extars/locus/b_debug_transforms: False
 * /extars/locus/b_enable_computation_time_profiling: True
 * /extars/locus/b_enable_msw: True
 * /extars/locus/b_integrate_interpolated_odom: True
 * /extars/locus/b_is_flat_ground_assumption: False
 * /extars/locus/b_pub_odom_on_timer: False
 * /extars/locus/b_publish_xy_cross_section: True
 * /extars/locus/b_run_with_gt_point_cloud: False
 * /extars/locus/b_sub_to_lsm: False
 * /extars/locus/b_verbose: False
 * /extars/locus/box_filter_size: 20
 * /extars/locus/buffers/imu_buffer_size_limit: 100
 * /extars/locus/buffers/odometry_buffer_size_limit: 100
 * /extars/locus/data_integration/mode: 0
 * /extars/locus/filtering/decimate_percentage: 0.9
 * /extars/locus/filtering/decimate_percentage_open_space: 0.93
 * /extars/locus/filtering/extract_features: False
 * /extars/locus/filtering/grid_filter: False
 * /extars/locus/filtering/grid_res: 0.2
 * /extars/locus/filtering/outlier_filter: False
 * /extars/locus/filtering/outlier_knn: 10
 * /extars/locus/filtering/outlier_std: 1.0
 * /extars/locus/filtering/radius: 0.15
 * /extars/locus/filtering/radius_filter: False
 * /extars/locus/filtering/radius_knn: 3
 * /extars/locus/filtering/random_filter: False
 * /extars/locus/frame_conversions/b_convert_imu_to_base_link_frame: True
 * /extars/locus/frame_id/artifacts_in_global: True
 * /extars/locus/frame_id/base: extars/base_link
 * /extars/locus/frame_id/bd_odometry: extars/odom
 * /extars/locus/frame_id/fixed: extars/map
 * /extars/locus/frame_id/imu: extars/imu_link
 * /extars/locus/gt_point_cloud_filename: /home/costar/data...
 * /extars/locus/icp/corr_dist: 1.0
 * /extars/locus/icp/enable_timing_output: False
 * /extars/locus/icp/iterations: 20
 * /extars/locus/icp/max_rotation: 1.0
 * /extars/locus/icp/max_translation: 1.0
 * /extars/locus/icp/num_threads: 4
 * /extars/locus/icp/recompute_covariances: False
 * /extars/locus/icp/registration_method: gicp
 * /extars/locus/icp/tf_epsilon: 0.001
 * /extars/locus/icp/transform_thresholding: True
 * /extars/locus/localization/compute_icp_covariance: True
 * /extars/locus/localization/compute_icp_observability: False
 * /extars/locus/localization/corr_dist: 0.2
 * /extars/locus/localization/enable_timing_output: False
 * /extars/locus/localization/icp_covariance_method: 1
 * /extars/locus/localization/icp_max_covariance: 0.01
 * /extars/locus/localization/iterations: 20
 * /extars/locus/localization/max_rotation: 1.0
 * /extars/locus/localization/max_translation: 1.0
 * /extars/locus/localization/normal_search_radius: 10
 * /extars/locus/localization/num_threads: 4
 * /extars/locus/localization/recompute_covariance_local_map: False
 * /extars/locus/localization/recompute_covariance_scan: False
 * /extars/locus/localization/registration_method: gicp
 * /extars/locus/localization/tf_epsilon: 1e-05
 * /extars/locus/localization/transform_thresholding: True
 * /extars/locus/map/b_publish_map_info: False
 * /extars/locus/map/b_publish_only_with_subscribers: True
 * /extars/locus/map/default_map_buffer_max_size: 7
 * /extars/locus/map/octree_resolution: 0.001
 * /extars/locus/map/volume_voxel_size: 1.024
 * /extars/locus/map_publishment/b_publish_map: True
 * /extars/locus/map_publishment/meters: 1
 * /extars/locus/mapper/num_threads: 4
 * /extars/locus/number_of_points_open_space: 9999999
 * /extars/locus/odom_pub_rate: 10
 * /extars/locus/points_to_process_in_callback: 3000
 * /extars/locus/publish_diagnostics: True
 * /extars/locus/queues/imu_queue_size: 100
 * /extars/locus/queues/lidar_queue_size: 1
 * /extars/locus/queues/odom_queue_size: 100
 * /extars/locus/robot_name: extars
 * /extars/locus/rotation_threshold_closed_space_kf: 0.3
 * /extars/locus/rotation_threshold_kf: 0.3
 * /extars/locus/rotation_threshold_open_space_kf: 0.6
 * /extars/locus/rotational_velocity_threshold: 1
 * /extars/locus/sensor_health_timeout: 0.4
 * /extars/locus/statistics_time_window: 5
 * /extars/locus/statistics_verbosity_level: low
 * /extars/locus/tf_prefix: extars
 * /extars/locus/transform_wait_duration: 0.01
 * /extars/locus/translation_threshold_closed_space_kf: 1.0
 * /extars/locus/translation_threshold_kf: 1.0
 * /extars/locus/translation_threshold_msw: 5
 * /extars/locus/translation_threshold_open_space_kf: 2.0
 * /extars/locus/translational_velocity_threshold: 0.1
 * /extars/locus/velocity_buffer_size: 10
 * /extars/locus/wait_for_odom_transform_timeout: 0.1
 * /extars/locus/window_local_mapping_type: mapper
 * /extars/locus/xy_cross_section_threshold: 2500
 * /extars/normal_computation/num_threads: 4
 * /extars/transform_points_base_link/filter_field_name: z
 * /extars/transform_points_base_link/filter_limit_max: 100
 * /extars/transform_points_base_link/filter_limit_min: -100
 * /extars/transform_points_base_link/output_frame: extars/base_link
 * /extars/voxel_grid/filter_field_name: z
 * /extars/voxel_grid/filter_limit_max: 100
 * /extars/voxel_grid/filter_limit_min: -100
 * /extars/voxel_grid/filter_limit_negative: False
 * /extars/voxel_grid/leaf_size: 0.25
 * /extars/voxel_grid/output_frame: extars/base_link
 * /rosdistro: melodic
 * /rosversion: 1.14.13

NODES
  /
    odometry_republish (locus/odometry_republish.py)
    vectornav_republish (locus/vectornav_republish.py)
  /extars/
    body_filter (nodelet/nodelet)
    locus (locus/locus_node)
    nodelet_manager (nodelet/nodelet)
    normal_computation (nodelet/nodelet)
    transform_points_base_link (nodelet/nodelet)
    voxel_grid (nodelet/nodelet)
  /extars/extars/
    robot_state_publisher_extars_28081_4633833944357679098 (robot_state_publisher/robot_state_publisher)

ROS_MASTER_URI=http://localhost:11311

process[extars/extars/robot_state_publisher_extars_28081_4633833944357679098-1]: started with pid [28117]
process[extars/locus-2]: started with pid [28118]
process[extars/transform_points_base_link-3]: started with pid [28119]
process[extars/nodelet_manager-4]: started with pid [28126]
process[extars/body_filter-5]: started with pid [28129]
process[extars/voxel_grid-6]: started with pid [28134]
process[extars/normal_computation-7]: started with pid [28135]
process[vectornav_republish-8]: started with pid [28141]
[ INFO] [1658497761.825094395]: Loading nodelet /extars/voxel_grid of type point_cloud_filter/CustomVoxelGrid to manager nodelet_manager with the following remappings:
[ INFO] [1658497761.825719007]: /extars/voxel_grid/input -> /extars/body_filter/output
[ INFO] [1658497761.826968117]: waitForService: Service [/extars/nodelet_manager/load_nodelet] has not been advertised, waiting...
process[odometry_republish-9]: started with pid [28157]
[ INFO] [1658497761.847798486]: waitForService: Service [/extars/nodelet_manager/load_nodelet] is now available.
[ INFO] [1658497761.893490299]: Locus::Initialize
[ INFO] [1658497761.906050224]: PointCloudOdometry - Initialize
[ INFO] [1658497761.906096521]: PointCloudOdometry - LoadParameters
[ WARN] [1658497761.907359380]: /extars/locus: Failed to search for parameter 'fiducial_calibration/position/x'.
[ WARN] [1658497761.907754290]: /extars/locus: Failed to search for parameter 'fiducial_calibration/position/y'.
[ WARN] [1658497761.908131623]: /extars/locus: Failed to search for parameter 'fiducial_calibration/position/z'.
[ WARN] [1658497761.908492397]: /extars/locus: Failed to search for parameter 'fiducial_calibration/orientation/x'.
[ WARN] [1658497761.908852964]: /extars/locus: Failed to search for parameter 'fiducial_calibration/orientation/y'.
[ WARN] [1658497761.909207968]: /extars/locus: Failed to search for parameter 'fiducial_calibration/orientation/z'.
[ WARN] [1658497761.909582912]: /extars/locus: Failed to search for parameter 'fiducial_calibration/orientation/w'.
[ WARN] [1658497761.923560601]: Can't find fiducials, using origin
[ INFO] [1658497761.923600350]: PointCloudOdometry - RegisterCallbacks
[ INFO] [1658497761.928339872]: PointCloudOdometry - SetupICP
[ INFO] [1658497761.928374459]: RegistrationMethod::GICP activated.
[ INFO] [1658497761.928437935]: GICP
[ INFO] [1658497761.928463325]: getMaxCorrespondenceDistance: 1
[ INFO] [1658497761.928479733]: getMaximumIterations: 20
[ INFO] [1658497761.928499676]: getTransformationEpsilon: 0.001
[ INFO] [1658497761.928516765]: getEuclideanFitnessEpsilon: 0.005
[ INFO] [1658497761.928531274]: Ransac: 0
[ INFO] [1658497761.928545951]: getRANSACIterations: 0
[ INFO] [1658497761.928561101]:  ROTATION EPSILION: 0.002
[ INFO] [1658497761.928577956]: getCorrespondenceRandomness:20
[ INFO] [1658497761.928591456]: getMaximumOptimizerIterations:20
[ INFO] [1658497761.928606934]: getConvergeCriteria:0x5601f407b460
[ INFO] [1658497761.928622116]: RANSACOutlie: 0.05
[ INFO] [1658497761.928635933]: CLASS NAME: MultithreadedGeneralizedIterativeClosestPoint
[ INFO] [1658497761.928666491]: PointCloudLocalization - LoadParameters
[ WARN] [1658497761.931287005]: /extars/locus: Failed to search for parameter 'fiducial_calibration/position/x'.
[ WARN] [1658497761.931646403]: /extars/locus: Failed to search for parameter 'fiducial_calibration/position/y'.
[ WARN] [1658497761.932002469]: /extars/locus: Failed to search for parameter 'fiducial_calibration/position/z'.
[ WARN] [1658497761.932341422]: /extars/locus: Failed to search for parameter 'fiducial_calibration/orientation/x'.
[ WARN] [1658497761.932700739]: /extars/locus: Failed to search for parameter 'fiducial_calibration/orientation/y'.
[ WARN] [1658497761.933046714]: /extars/locus: Failed to search for parameter 'fiducial_calibration/orientation/z'.
[ WARN] [1658497761.933393085]: /extars/locus: Failed to search for parameter 'fiducial_calibration/orientation/w'.
[ WARN] [1658497761.950052801]: Can't find fiducials, using origin
[ INFO] [1658497761.954335861]: PointCloudLocalization - SetupICP
[ INFO] [1658497761.954417290]: RegistrationMethod::GICP activated.
[ INFO] [1658497761.954486440]: GICP activated.
[ INFO] [1658497761.954539493]: MaxCorrespondenceDistance: 0.2
[ INFO] [1658497761.954584835]: MaximumIterations: 20
[ INFO] [1658497761.954630136]: TransformationEpsilon: 1e-05
[ INFO] [1658497761.954674394]: EuclideanFitnessEpsilon: 0.01
[ INFO] [1658497761.954718245]: RANSACIterations: 0
[ INFO] [1658497761.954766369]: RotationEpsilon 0.002
[ INFO] [1658497761.954809102]: CorrespondenceRandomness:20
[ INFO] [1658497761.954851512]: MaximumOptimizerIterations:50
[ INFO] [1658497761.954895828]: ConvergeCriteria:0x5601f40844e0
[ INFO] [1658497761.954940364]: RANSACOutlierRejectionThreshold: 0.05
[ INFO] [1658497761.954984364]: Locus::LoadParameters
[ INFO] [1658497761.999801117]: b_integrate_interpolated_odom_: 1
[ INFO] [1658497761.999894934]: MappingMethod::MAPPER activated.
[ INFO] [1658497761.999963894]: Setting up number threads for local mapping: 4
[ INFO] [1658497762.015930592]: No integration requested
[ INFO] [1658497762.015969254]: Locus::RegisterCallbacks
[ INFO] [1658497762.026594844]: Locus::CreatePublishers
[ INFO] [1658497762.033209693]: Locus::LoadCalibrationFromTfTree
[ INFO] [1658497762.133604480]: Loaded pose_sensor to imu calibration B_T_L:
 -0.19
     0
-0.149
1 0 0
0 1 0
0 0 1
[ INFO] [1658497762.133705735]: q: x: 0.000, y: 0.000, z: 0.000, w: 1.000
[ INFO] [1658497762.137128002]: [Locus::setAsyncSpinners] : New subscriber for IMU
[ INFO] [1658497762.138932859]: [Locus::setAsyncSpinners] : New subscriber for odom
[ INFO] [1658497762.140212971]: [Locus::setAsyncSpinners] : New subscriber for lidar
[INFO] [1658497762.262007]: waiting for subscribed topics to be ready
[INFO] [1658497762.279811]: waiting for subscribed topics to be ready
Failed to find match for field 'normal_x'.
Failed to find match for field 'normal_y'.
Failed to find match for field 'normal_z'.
Failed to find match for field 'curvature'.
locus_node: /build/pcl-OilVEB/pcl-1.8.1+dfsg1/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp:136: int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch(const PointT&, int, std::vector<int>&, std::vector<float>&) const [with PointT = pcl::PointXYZINormal; Dist = flann::L2_Simple<float>]: Assertion `point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"' failed.
locus_node: /build/pcl-OilVEB/pcl-1.8.1+dfsg1/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp:136: int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch(const PointT&, int, std::vector<int>&, std::vector<float>&) const [with PointT = pcl::PointXYZINormal; Dist = flann::L2_Simple<float>]: Assertion `point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"' failed.
locus_node: /build/pcl-OilVEB/pcl-1.8.1+dfsg1/kdtree/include/pcl/kdtree/impl/kdtree_flann.hpp:136: int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch(const PointT&, int, std::vector<int>&, std::vector<float>&) const [with PointT = pcl::PointXYZINormal; Dist = flann::L2_Simple<float>]: Assertion `point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"' failed.
[extars/locus-2] process has died [pid 28118, exit code -6, cmd /home/ctrazzi/locus_ws/devel/lib/locus/locus_node ~LIDAR_TOPIC:=/velodyne_points ~IMU_TOPIC:=/vectornav_base_link ~SPACE_MONITOR_TOPIC:=/localizer_space_monitor/xy_cross_section __name:=locus __log:=/var/log/extars/ros/6dabe0b2-09a6-11ed-bbec-54b203083f30/extars-locus-2.log].
log file: /var/log/extars/ros/6dabe0b2-09a6-11ed-bbec-54b203083f30/extars-locus-2*.log
[INFO] [1658497766.283824]: republish measure vectornav
[INFO] [1658497767.265861]: republish measure odometry
^C[odometry_republish-9] killing on exit
[vectornav_republish-8] killing on exit
[extars/normal_computation-7] killing on exit
[extars/voxel_grid-6] killing on exit
[extars/body_filter-5] killing on exit
[ INFO] [1658497783.787002610]: Unloading nodelet /extars/voxel_grid from manager nodelet_manager
[extars/transform_points_base_link-3] killing on exit
[extars/extars/robot_state_publisher_extars_28081_4633833944357679098-1] killing on exit
[extars/nodelet_manager-4] killing on exit
Exception in thread Thread-8:
Traceback (most recent call last):
  File "/usr/lib/python2.7/threading.py", line 801, in __bootstrap_inner
    self.run()
  File "/usr/lib/python2.7/threading.py", line 754, in run
    self.__target(*self.__args, **self.__kwargs)
  File "/home/ctrazzi/locus_ws/src/LOCUS/locus/scripts/vectornav_republish.py", line 47, in send_measure
    imu_pub.publish(vn_imu)
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/topics.py", line 882, in publish
    self.impl.publish(data)
  File "/opt/ros/melodic/lib/python2.7/dist-packages/rospy/topics.py", line 1041, in publish
    raise ROSException("publish() to a closed topic")
ROSException: publish() to a closed topic

shutting down processing monitor...
... shutting down processing monitor complete
done

And when I try to run it following your example I get:

~/locus_ws/src/LOCUS/tmuxp_config$ tmuxp load extars_sanpretronilla_run.yaml 
Traceback (most recent call last):
  File "/usr/bin/tmuxp", line 11, in <module>
    load_entry_point('tmuxp==1.3.5', 'console_scripts', 'tmuxp')()
  File "/home/ctrazzi/.local/lib/python3.6/site-packages/pkg_resources/__init__.py", line 479, in load_entry_point
    return get_distribution(dist).load_entry_point(group, name)
  File "/home/ctrazzi/.local/lib/python3.6/site-packages/pkg_resources/__init__.py", line 2861, in load_entry_point
    return ep.load()
  File "/home/ctrazzi/.local/lib/python3.6/site-packages/pkg_resources/__init__.py", line 2465, in load
    return self.resolve()
  File "/home/ctrazzi/.local/lib/python3.6/site-packages/pkg_resources/__init__.py", line 2471, in resolve
    module = __import__(self.module_name, fromlist=['__name__'], level=0)
  File "/usr/lib/python3/dist-packages/tmuxp/__init__.py", line 20, in <module>
    from . import config, util, cli
  File "/usr/lib/python3/dist-packages/tmuxp/cli.py", line 550, in <module>
    help='Like -2, but indicates that the terminal supports 88 colours.')
  File "/home/ctrazzi/.local/lib/python3.6/site-packages/click/decorators.py", line 223, in decorator
    _param_memo(f, ArgumentClass(param_decls, **attrs))
  File "/home/ctrazzi/.local/lib/python3.6/site-packages/click/core.py", line 2905, in __init__
    super().__init__(param_decls, required=required, **attrs)
  File "/home/ctrazzi/.local/lib/python3.6/site-packages/click/core.py", line 2030, in __init__
    param_decls or (), expose_value
  File "/home/ctrazzi/.local/lib/python3.6/site-packages/click/core.py", line 2941, in _parse_decls
    "Arguments take exactly one parameter declaration, got"
TypeError: Arguments take exactly one parameter declaration, got 2.

Can you please help me to make it work? Can it be realated to the no dense pointcloud?

femust commented 2 years ago

Hey @Cristian-wp

Thanks for testing and trying out our solution!

At the first sight, it seems that it's related to your pc setup and it's weird that sometimes you run python2 and python3. It may be causing some problems.

What I suggest you could do, without fixing the local problems, is go on branch feature/docker here and follow README steps. If you run docker, by default is connected with "your host network" so if you run roscore locally the docker sees that and you may run locus. Let me know if that helps you!

Cristian-wp commented 2 years ago

I have format my pc and now it start.