Skip to content

Commit

Permalink
get running with Spot dataset
Browse files Browse the repository at this point in the history
  • Loading branch information
Sangwoo Moon committed Jun 3, 2022
1 parent be84e41 commit aab5a59
Show file tree
Hide file tree
Showing 4 changed files with 169 additions and 77 deletions.
55 changes: 9 additions & 46 deletions sensor_description/config/spot1_sensors.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -34,30 +34,6 @@ realsense_right:
p: 0
y: ${-pi/2}
rate: 10
realsense_rear:
name: camera_rear
parent: base_link
position:
x: -0.475
y: 0.015
z: 0.080
orientation:
r: 0
p: 0
y: ${pi}
rate: 10
realsense_up:
name: camera_up
parent: base_link
position:
x: 0
y: 0.361805
z: 0.359793
orientation:
r: 0
p: ${-pi/6}
y: 0
rate: 10
boson:
name: boson
parent: base_link
Expand All @@ -74,41 +50,28 @@ velodyne:
name: velodyne
parent: base_link
position:
x: 0.265
y: 0.003
z: 0.26
x: 0.290
y: 0
z: 0.343
orientation:
r: 0.008
p: -0.023
y: -0.007
r: 0
p: 0
y: -0.037
rate: 10
topic_name: velodyne_points
vn100:
name: vn100
parent: velodyne_base_link
parent: base_link
topic_name: vn100/imu
position:
x: -0.064254
y: 0
z: -0.00635
orientation:
r: 0
p: 0
y: 0
rate: 50
respeaker:
name: respeaker
parent: velodyne_base_link
topic_name: audio
position:
x: 0
y: 0
z: 0.08
z: 0.2
orientation:
r: 0
p: 0
y: ${pi/2}
rate: 1
rate: 50
realsense_builtin_frontleft:
name: builtin_camera_frontleft
parent: base_link
Expand Down
62 changes: 32 additions & 30 deletions sensor_description/urdf/robot_description.urdf.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -32,37 +32,39 @@
${sensor_props['velodyne']['orientation']['y']}" />
</xacro:VLP-16>

<xacro:VLP-16
parent="${sensor_props['velodyne_rear']['parent']}"
name="${sensor_props['velodyne_rear']['name']}"
topic="${sensor_props['velodyne_rear']['topic_name']}"
hz="${sensor_props['velodyne_rear']['rate']}"
samples="440"
noise="0.016"
gpu="true">
<origin xyz="${sensor_props['velodyne_rear']['position']['x']}
${sensor_props['velodyne_rear']['position']['y']}
${sensor_props['velodyne_rear']['position']['z']}"
rpy="${sensor_props['velodyne_rear']['orientation']['r']}
${sensor_props['velodyne_rear']['orientation']['p']}
${sensor_props['velodyne_rear']['orientation']['y']}" />
</xacro:VLP-16>
<xacro:if value="$(eval 'husky' in robot_namespace)">
<xacro:VLP-16
parent="${sensor_props['velodyne_rear']['parent']}"
name="${sensor_props['velodyne_rear']['name']}"
topic="${sensor_props['velodyne_rear']['topic_name']}"
hz="${sensor_props['velodyne_rear']['rate']}"
samples="440"
noise="0.016"
gpu="true">
<origin xyz="${sensor_props['velodyne_rear']['position']['x']}
${sensor_props['velodyne_rear']['position']['y']}
${sensor_props['velodyne_rear']['position']['z']}"
rpy="${sensor_props['velodyne_rear']['orientation']['r']}
${sensor_props['velodyne_rear']['orientation']['p']}
${sensor_props['velodyne_rear']['orientation']['y']}" />
</xacro:VLP-16>

<xacro:VLP-16
parent="${sensor_props['velodyne_front']['parent']}"
name="${sensor_props['velodyne_front']['name']}"
topic="${sensor_props['velodyne_front']['topic_name']}"
hz="${sensor_props['velodyne_front']['rate']}"
samples="440"
noise="0.016"
gpu="true">
<origin xyz="${sensor_props['velodyne_front']['position']['x']}
${sensor_props['velodyne_front']['position']['y']}
${sensor_props['velodyne_front']['position']['z']}"
rpy="${sensor_props['velodyne_front']['orientation']['r']}
${sensor_props['velodyne_front']['orientation']['p']}
${sensor_props['velodyne_front']['orientation']['y']}" />
</xacro:VLP-16>
<xacro:VLP-16
parent="${sensor_props['velodyne_front']['parent']}"
name="${sensor_props['velodyne_front']['name']}"
topic="${sensor_props['velodyne_front']['topic_name']}"
hz="${sensor_props['velodyne_front']['rate']}"
samples="440"
noise="0.016"
gpu="true">
<origin xyz="${sensor_props['velodyne_front']['position']['x']}
${sensor_props['velodyne_front']['position']['y']}
${sensor_props['velodyne_front']['position']['z']}"
rpy="${sensor_props['velodyne_front']['orientation']['r']}
${sensor_props['velodyne_front']['orientation']['p']}
${sensor_props['velodyne_front']['orientation']['y']}" />
</xacro:VLP-16>
</xacro:if>

<!-- VN100 -->
<link name="imu_link"/>
Expand Down
127 changes: 127 additions & 0 deletions tmuxp_config/nebula_odometry_datasets/E_Spot1_Valentine1.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,127 @@
session_name: locus_dataset

environment:
# Path for the dataset
DATA_PATH: /home/costar/data/E_Spot1_Valentine1
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: "2"
# Number of lidars to use (if available)
NUM_VLP: "1"

# Change the robot name and type to match the dataset
ROBOT_NAME: spot1
ROBOT_TYPE: spot
# 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: "1600"
# 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: "1550"
# 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;

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;

# 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



- 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 \
2 changes: 1 addition & 1 deletion tmuxp_config/run_locus.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ windows:
fi;

# launch rviz
- rviz -d $(rospack find locus)/rviz/locus.rviz
- rviz -d $(rospack find locus)/rviz/$ROBOT_NAME_locus.rviz

# Front-end
- if [ $ROBOT_TYPE = 'husky' ]; then
Expand Down

0 comments on commit aab5a59

Please sign in to comment.