LOCUS
LOCUS copied to clipboard
Published
20 hours ago •
NeBula-Autonomy
NeBula-Autonomy
Can not run locus with custom dataset
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.
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?
