Can not run locus with custom dataset

[Cristian-wp]

Hello, I have finally manage to run your example dataset.
Now I am trying to run mine, but I can not remap the topic correctly.

When I launch my modified tmux scrip I get the following output from the two terminals:
first terminal:

 rosparam set /use_sim_time true
 sleep $TIME_TO_MAP_SAVE; rosrun pcl_ros pointcloud_to_pcd input:=/$ROBOT_NAME/locus/octree_map _prefix:=$DATA_PATH/locus_$RUN_NUMBER/map_locus
ctrazzi@extars:~/locus_ws/src/LOCUS/tmuxp_config$  rosparam set /use_sim_time true
ctrazzi@extars:~/locus_ws/src/LOCUS/tmuxp_config$  sleep $TIME_TO_MAP_SAVE; rosrun pcl_ros pointcloud_to_pcd input:=/$ROBOT_NAME/locus/octree_map _prefix:=$DATA_PATH/locus_$RUN_NUMBER/map_locus

second terminal:

 rosparam set /use_sim_time true
ctrazzi@extars:~/locus_ws/src/LOCUS/tmuxp_config$  rosparam set /use_sim_time true

This is the rviz output:

And this is the rqt_graph:

I think this is a remapping problem, because the rosbag topics are non connected to locus.

This is my run_locus.yaml

session_name: locus_dataset

environment:
  # Path for the dataset
  DATA_PATH: /home/ctrazzi/data/Sanp
  DATA_FOLDER: rosbag/
  # Change the run number to save the output data in a different folder
  RUN_NUMBER: "test_01"

  # version of logging. If unsure, select 2 (the latest). 1 is for tunnel datasets
  LOG_VERSION: "3" 
  # Number of lidars to use (if available)
  NUM_VLP: "1"

  # Change the robot name and type to match the dataset 
  ROBOT_NAME: extars
  ROBOT_TYPE: extars
  # If unsure, leave as base1
  BASE_NAME: base1
  
  # Rosbag parameters
  START: "0"
  RATE: "1.0"

  # See https://github.com/NeBula-Autonomy/nebula-odometry-dataset/blob/main/pages/dataset.md 
  # for the expected length of datasets
  # This param can be used to automatically close the session at the end of the run (currently disabled)
  TIME_TO_END: "600"
  # This param is used to start saving the map near the end of the run (try 1 minute before the end)
  TIME_TO_MAP_SAVE: "500"
  # The topic for the odometry to record
  ODOM: locus/odometry


options:
  default-command: /bin/bash

windows:
- window_name: locus  
  focus: true  
  layout: tiled  
  shell_command_before:
    - rosparam set /use_sim_time true   
    # Set up the parameter files from the GT products
    - cp $DATA_PATH/fiducial_calibration_$ROBOT_NAME.yaml ~/.ros/
    - if [ $ROBOT_TYPE = 'husky' ]; then
      cp $DATA_PATH/$ROBOT_NAME*.yaml $(rospack find sensor_description)/config/ ;
      fi;
    - if [ $ROBOT_TYPE = 'spot' ]; then
      cp $DATA_PATH/$ROBOT_NAME*.yaml $(rospack find sensor_description)/config/ ;
      fi;
    - if [ $ROBOT_TYPE = 'extars' ]; then
      cp $DATA_PATH/$ROBOT_NAME*.yaml $(rospack find sensor_description)/config/ ;
      fi;      

  panes:

    ################################### RUN ###################################

    # Bags and topics of interest (switching if the log versions is older)
    - if [ $LOG_VERSION -eq 2 ]; then
      sleep 10; rosbag play $DATA_PATH/$DATA_FOLDER*.bag -s$START -r$RATE --clock  --topics clock:=/clock 
      /$ROBOT_NAME/velodyne_points /$ROBOT_NAME/velodyne_front/velodyne_points /$ROBOT_NAME/velodyne_rear/velodyne_points 
      /$ROBOT_NAME/hero/wio_ekf/odom /$ROBOT_NAME/vn100/imu_wori_wcov /$ROBOT_NAME/visual_odom 
      /$ROBOT_NAME/wheel_odom /$ROBOT_NAME/hvm/lidar/points /$ROBOT_NAME/hvm/odometry
      /$ROBOT_NAME/unreconciled_artifact /$ROBOT_NAME/uwb_frontend/range_measurements ;
      fi;
      if [ $LOG_VERSION -eq 1 ]; then
      sleep 10;rosbag play $DATA_PATH/$DATA_FOLDER*.bag -s$START -r$RATE --clock  --prefix $ROBOT_NAME/ 
      --topics clock:=/clock velodyne_points velodyne_front/velodyne_points /velodyne_rear/velodyne_points 
      hero/wio_ekf/odom vn100/imu_wori_wcov vn100/imu visual_odom /tf_static ;
      fi;
    - if [ $LOG_VERSION -eq 3 ]; then
      rosbag play $DATA_PATH/$DATA_FOLDER/test_manuale.bag ;
      fi;      

    # launch rviz
    - rviz -d $(rospack find locus)/rviz/$(echo $ROBOT_NAME)_locus.rviz

    # Front-end
    - if [ $ROBOT_TYPE = 'husky' ]; then
      roslaunch locus locus.launch robot_namespace:=$ROBOT_NAME number_of_velodynes:=$NUM_VLP;
      fi;
      if [ $ROBOT_TYPE = 'spot' ]; then
      roslaunch locus locus.launch robot_namespace:=$ROBOT_NAME;
      fi
    - if [ $ROBOT_TYPE = 'extars' ]; then
      roslaunch locus locus_extars.launch robot_namespace:=$ROBOT_NAME number_of_velodynes:=$NUM_VLP;
      fi;



- window_name: record_at_end  
  focus: false  
  layout: tiled  
  shell_command_before:
    - rosparam set /use_sim_time true   

  panes:
    # Map
    # Front-End Map
    - sleep $TIME_TO_MAP_SAVE; rosrun pcl_ros pointcloud_to_pcd input:=/$ROBOT_NAME/locus/octree_map _prefix:=$DATA_PATH/locus_$RUN_NUMBER/map_locus 
    
    # ..
    - 


- window_name: record  
  focus: false  
  layout: tiled  
  shell_command_before:
    - mkdir $DATA_PATH/locus_$RUN_NUMBER
    - rosparam set /use_sim_time true   

  panes:

    ################################### RECORD ###################################

    # Odometry 
    - rosbag record -O $DATA_PATH/locus_$RUN_NUMBER/odometry.bag /$ROBOT_NAME/$ODOM /$ROBOT_NAME/locus/localization_integrated_estimate /$ROBOT_NAME/locus/odometry_integrated_estimate /$ROBOT_NAME/locus/localization_incremental_estimate /$ROBOT_NAME/locus/odometry_incremental_estimate  
    - rosbag record -O $DATA_PATH/locus_$RUN_NUMBER/stationary.bag /$ROBOT_NAME/stationary_accel

    # Odometry Rate and Delay
    - rostopic hz /$ROBOT_NAME/$ODOM -w3 >> $DATA_PATH/locus_$RUN_NUMBER/rate.txt
    - rostopic delay /$ROBOT_NAME/$ODOM -w3 >> $DATA_PATH/locus_$RUN_NUMBER/delay.txt

    # Computation Time Profiling
    - rosbag record -O $DATA_PATH/locus_$RUN_NUMBER/lidar_callback_duration.bag /$ROBOT_NAME/locus/lidar_callback_duration
    - rosbag record -O $DATA_PATH/locus_$RUN_NUMBER/scan_to_scan_duration.bag /$ROBOT_NAME/locus/scan_to_scan_duration
    - rosbag record -O $DATA_PATH/locus_$RUN_NUMBER/scan_to_map_duration.bag /$ROBOT_NAME/locus/scan_to_submap_duration

    ################################ END RECORDING ###############################
    # Uncomment the line below to automatically end the run
    # - sleep $TIME_TO_END; rosnode kill -a 
    # Use the line below to prepare a kill command when you want to stop
    - rosnode kill -a \

And this is my locus.launch:

launch>
 
    <arg name="robot_namespace" default="extars"/>

    <arg name="use_gdb" default="true"/>
    <arg name="nodelet_manager" value="nodelet_manager"/>
    <arg name="launch_prefix" value="gdb -ex run --args" if="$(arg use_gdb)"/>
    <arg name="launch_prefix" value="" unless="$(arg use_gdb)"/>
    <arg name="nodelet_args" value="--no-bond"/>
    <arg name="respawn" value="false" if="$(arg use_gdb)"/>
    <arg name="respawn" value="true" unless="$(arg use_gdb)"/>
    <arg name="robot_type" value="extars" if="$(eval robot_namespace.startswith('extars'))"/>
    

    <arg name="number_of_velodynes" default="1" />
    <arg name="b_use_multiple_pc" value="$(eval arg('number_of_velodynes') > 1)"/>
    <arg name="pc_input" value="velodyne_points"/>
    
    <!-- 0:TOP, 1:FRONT, 2:REAR -->
     <arg name="pc_trans_in_0" default="velodyne_points/transformed"/>
     <arg name="pc_trans_in_1" default="velodyne_front/velodyne_points/transformed"/>
     <arg name="pc_trans_in_2" default="velodyne_rear/velodyne_points/transformed"/>


    <group ns="$(arg robot_namespace)">

        <!-- Load parameters -->
        <rosparam file="$(find locus)/config/body_filter_params_$(arg robot_type).yaml"
                    subst_value="true"/>

        <!-- Load robot description -->
        <include file="$(find locus)/launch/robot_description_extars.launch">
            <arg name="robot_namespace" value="$(arg robot_namespace)"/>
        </include>
   
        <node
            pkg="locus"
            name="locus"
            type="locus_node"
            output="screen">
            

            <remap from="~LIDAR_TOPIC" to="$(arg pc_input)"/>
            <remap from="~ODOMETRY_TOPIC" to="lvi_sam/vins/odometry/odometry" if="$(eval robot_namespace.startswith('extars'))"/>       

            <remap from="~IMU_TOPIC" to="vectornav/IMU"/>            
            <remap from="~POSE_TOPIC" to="not_currently_used"/>


    <remap from="~SPACE_MONITOR_TOPIC" to="localizer_space_monitor/xy_cross_section"/>

            <!-- For Sim -->
            <!-- <remap from="~ODOMETRY_TOPIC" to="wheel_odom"/> -->         

            <remap from="/diagnostics" to="/diagnostics"/>

            <param name="robot_name" value="$(arg robot_namespace)"/>
            <param name="tf_prefix" value="$(arg robot_namespace)"/>

            <param name="b_integrate_interpolated_odom" value="false" if="$(eval robot_namespace.startswith('extars'))"/>

            <rosparam file="$(find locus)/config/lo_settings_extars.yaml" />

            <param name="b_pub_odom_on_timer" value="false" if="$(eval robot_namespace.startswith('extars'))"/>

            <rosparam file="$(find locus)/config/lo_frames_extars.yaml" subst_value="true"/>
            <rosparam file="$(find point_cloud_filter)/config/parameters_extars.yaml"/>
            <rosparam file="$(find point_cloud_odometry)/config/parameters_extars.yaml"/> 
            
            <rosparam file="$(find point_cloud_localization)/config/parameters_extars.yaml" if="$(eval robot_type == 'extars')"/>
            
            <rosparam file="$(find point_cloud_mapper)/config/parameters_extars.yaml"/>

            <param name="localization/num_threads"       value="4" if="$(eval robot_namespace.startswith('extars'))" />
            <param name="icp/num_threads"                value="4" if="$(eval robot_namespace.startswith('extars'))" />
            <param name="mapper/num_threads"             value="4" if="$(eval robot_namespace.startswith('extars'))" />

        </node>

        <node pkg="locus" name="sensors_health_monitor" type="sensors_health_monitor.py" output="screen" if="$(eval number_of_velodynes > 1)"> 
            <remap from="failure_detection" to="point_cloud_merger_lo/failure_detection"/>
            <remap from="resurrection_detection" to="point_cloud_merger_lo/resurrection_detection"/>
        </node>

        <node pkg="nodelet"
            type="nodelet"
            name="transform_points_base_link"
            args="standalone pcl/PassThrough">
            <remap from="~input" to="velodyne_points"/>
            <remap from="~output" to="$(arg pc_trans_in_0)"/>
            <rosparam subst_value="true">
                filter_field_name: z
                filter_limit_min: -100
                filter_limit_max: 100
                output_frame: $(arg robot_namespace)/base_link
            </rosparam>
        </node>

        <node if="$(eval arg('number_of_velodynes') > 1)"
            pkg="nodelet"
            type="nodelet"
            name="transform_points_base_link_front"
            args="standalone pcl/PassThrough">
            <remap from="~input" to="velodyne_front/velodyne_points"/>
            <remap from="~output" to="$(arg pc_trans_in_1)"/>
            <rosparam subst_value="true">
                filter_field_name: z
                filter_limit_min: -100
                filter_limit_max: 100
                output_frame: $(arg robot_namespace)/base_link
            </rosparam>
        </node>

        <node if="$(eval arg('number_of_velodynes') > 2)"
            pkg="nodelet"
            type="nodelet"
            name="transform_points_base_link_rear"
            args="standalone pcl/PassThrough">
            <remap from="~input" to="velodyne_rear/velodyne_points"/>
            <remap from="~output" to="$(arg pc_trans_in_2)"/>
            <rosparam subst_value="true">
                filter_field_name: z
                filter_limit_min: -100
                filter_limit_max: 100
                output_frame: $(arg robot_namespace)/base_link
            </rosparam>
        </node>

        <node if="$(arg b_use_multiple_pc)" pkg="point_cloud_merger" type="point_cloud_merger_node" name="point_cloud_merger_lo" output="screen">
            <rosparam file="$(find point_cloud_merger)/config/parameters.yaml"/>
            <param name="merging/number_of_velodynes" value="$(arg number_of_velodynes)"/>
            <remap from="~pcld0" to="$(arg pc_trans_in_0)"/>
            <remap from="~pcld1" to="$(arg pc_trans_in_1)"/>
            <remap from="~pcld2" to="$(arg pc_trans_in_2)"/>
            <remap from="~combined_point_cloud" to="combined_point_cloud"/>
        </node>    

        <node pkg="nodelet"
              type="nodelet"
              name="$(arg nodelet_manager)"
              launch-prefix="$(arg launch_prefix)"
              args="manager"
              respawn="$(arg respawn)"/>
        
        <node pkg="nodelet"
              type="nodelet"
              name="body_filter"
              args="load point_cloud_filter/BodyFilter $(arg nodelet_manager) $(arg nodelet_args)"
              respawn="$(arg respawn)">
          <remap from="~input" to="combined_point_cloud" if="$(arg b_use_multiple_pc)"/>
          <remap from="~input" to="velodyne_points/transformed" unless="$(arg b_use_multiple_pc)"/>
        </node>

<!-- old way. TODO: removing after locus testing-->
<!--node pkg="nodelet" type="nodelet" name="voxel_grid" args="load pcl/VoxelGrid $(arg nodelet_manager)" output="screen" respawn="true"-->

        <node pkg="nodelet" type="nodelet" name="voxel_grid" args="load point_cloud_filter/CustomVoxelGrid $(arg nodelet_manager)" output="screen" respawn="true">
            <remap from="~input" to="body_filter/output" />

            <rosparam subst_value="true">
            filter_field_name: z
            filter_limit_min: -100
            filter_limit_max: 100
            filter_limit_negative: False
            leaf_size: 0.25
            output_frame: $(arg robot_namespace)/base_link
            </rosparam>
        </node>

        <node pkg="nodelet"
            type="nodelet"
            name="normal_computation"
            args="load point_cloud_filter/NormalComputation $(arg nodelet_manager) $(arg nodelet_args)"
            respawn="$(arg respawn)">
            <remap from="~input" to="voxel_grid/output"/>
            <remap from="~output" to="$(arg pc_input)" />

           <param name="num_threads"                value="1" if="$(eval robot_namespace.startswith('extars'))" />
           

        </node>
      
    </group>
 
</launch>

Can you please help me?
Thanks in advance.

[femust]

Hey, @Cristian-wp thanks for testing our system!

Can you tell me whether you have some information in the console while running on your datasets? Like no odometry or something like this?