Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add a simple GNSS odometer #429

Open
wants to merge 4 commits into
base: ros2
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 10 additions & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ find_package(GTSAM REQUIRED)
find_package(Eigen REQUIRED)

include_directories(
include/lio_sam
include
)

rosidl_generate_interfaces(${PROJECT_NAME} "msg/CloudInfo.msg" "srv/SaveMap.srv" DEPENDENCIES std_msgs sensor_msgs)
Expand All @@ -52,6 +52,10 @@ ament_target_dependencies(${PROJECT_NAME}_mapOptimization rclcpp rclpy std_msgs
target_link_libraries(${PROJECT_NAME}_mapOptimization gtsam)
rosidl_target_interfaces(${PROJECT_NAME}_mapOptimization ${PROJECT_NAME} "rosidl_typesupport_cpp")

add_executable(${PROJECT_NAME}_simpleGpsOdom src/simpleGpsOdom.cpp)
ament_target_dependencies(${PROJECT_NAME}_simpleGpsOdom rclcpp rclpy std_msgs sensor_msgs geometry_msgs nav_msgs pcl_msgs visualization_msgs tf2 tf2_ros tf2_eigen tf2_sensor_msgs tf2_geometry_msgs OpenCV PCL)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

does not depend on e.g. OpenCV, PCL

rosidl_target_interfaces(${PROJECT_NAME}_simpleGpsOdom ${PROJECT_NAME} "rosidl_typesupport_cpp")

install(
DIRECTORY launch
DESTINATION share/${PROJECT_NAME}/
Expand Down Expand Up @@ -82,6 +86,11 @@ install(
DESTINATION lib/${PROJECT_NAME}
)

install(
TARGETS ${PROJECT_NAME}_simpleGpsOdom
DESTINATION lib/${PROJECT_NAME}
)

install(
DIRECTORY "include/"
DESTINATION include
Expand Down
98 changes: 98 additions & 0 deletions config/params_rs16.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,98 @@
/**:
ros__parameters:
use_sim_time: true

# Topics
pointCloudTopic: "/lidar/top/rslidar_sdk/rs/points" # Point cloud data
imuTopic: "/gnss/chc/imu" # IMU data
odomTopic: "odometry/imu" # IMU pre-preintegration odometry, same frequency as IMU
gpsTopic: "/gnss/fix" # GPS odometry topic from navsat, see module_navsat.launch file
gpsOdomTopic: "/gps_odom"
# Frames
lidarFrame: "lidar_link"
baselinkFrame: "base_link"
odometryFrame: "odom"
mapFrame: "map"

# GPS Settings
useImuHeadingInitialization: true # if using GPS data, set to "true"
useGpsElevation: true # if GPS elevation is bad, set to "false"
gpsCovThreshold: 2.0 # m^2, threshold for using GPS data
poseCovThreshold: 25.0 # m^2, threshold for using GPS data

# Export settings
savePCD: false # https://github.com/TixiaoShan/LIO-SAM/issues/3
savePCDDirectory: "/Downloads/LOAM/" # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation

# Sensor Settings
sensor: robosense # lidar sensor type, either 'velodyne', 'ouster' or 'livox' or 'robosense'
N_SCAN: 16 # number of lidar channels (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6)
Horizon_SCAN: 1800 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)
downsampleRate: 1 # default: 1. Downsample your data if too many
# points. i.e., 16 = 64 / 4, 16 = 16 / 1
lidarMinRange: 1.0 # default: 1.0, minimum lidar range to be used
lidarMaxRange: 1000.0 # default: 1000.0, maximum lidar range to be used

# IMU Settings
# imuAccNoise: 3.9939570888238808e-03
# imuGyrNoise: 1.5636343949698187e-03
# imuAccBiasN: 6.4356659353532566e-05
# imuGyrBiasN: 3.5640318696367613e-05
imuAccNoise: 5.9215155351791055e-03
imuGyrNoise: 1.5059072284923697e-03
imuAccBiasN: 1.3379378640306186e-04
imuGyrBiasN: 4.3430855283551206e-05

imuGravity: 9.80511
imuRPYWeight: 0.01

extrinsicTrans: [ -0.4, 0.14, 0.0 ]
extrinsicRot: [1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0 ]
extrinsicRPY: [ 1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0 ]

# LOAM feature threshold
edgeThreshold: 1.0
surfThreshold: 0.1
edgeFeatureMinValidNum: 10
surfFeatureMinValidNum: 100

# voxel filter paprams
odometrySurfLeafSize: 0.4 # default: 0.4 - outdoor, 0.2 - indoor
mappingCornerLeafSize: 0.2 # default: 0.2 - outdoor, 0.1 - indoor
mappingSurfLeafSize: 0.4 # default: 0.4 - outdoor, 0.2 - indoor

# robot motion constraint (in case you are using a 2D robot)
z_tollerance: 1000.0 # meters
rotation_tollerance: 1000.0 # radians

# CPU Params
numberOfCores: 4 # number of cores for mapping optimization
mappingProcessInterval: 0.15 # seconds, regulate mapping frequency

# Surrounding map
surroundingkeyframeAddingDistThreshold: 1.0 # meters, regulate keyframe adding threshold
surroundingkeyframeAddingAngleThreshold: 0.2 # radians, regulate keyframe adding threshold
surroundingKeyframeDensity: 2.0 # meters, downsample surrounding keyframe poses
surroundingKeyframeSearchRadius: 50.0 # meters, within n meters scan-to-map optimization
# (when loop closure disabled)

# Loop closure
loopClosureEnableFlag: true
loopClosureFrequency: 1.0 # Hz, regulate loop closure constraint add frequency
surroundingKeyframeSize: 50 # submap size (when loop closure enabled)
historyKeyframeSearchRadius: 15.0 # meters, key frame that is within n meters from
# current pose will be considerd for loop closure
historyKeyframeSearchTimeDiff: 30.0 # seconds, key frame that is n seconds older will be
# considered for loop closure
historyKeyframeSearchNum: 25 # number of hostory key frames will be fused into a
# submap for loop closure
historyKeyframeFitnessScore: 0.3 # icp threshold, the smaller the better alignment

# Visualization
globalMapVisualizationSearchRadius: 1000.0 # meters, global map visualization radius
globalMapVisualizationPoseDensity: 10.0 # meters, global map visualization keyframe density
globalMapVisualizationLeafSize: 1.0 # meters, global map visualization cloud density
71 changes: 58 additions & 13 deletions config/rviz2.rviz
Original file line number Diff line number Diff line change
Expand Up @@ -50,8 +50,8 @@ Visualization Manager:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: -0.8519169092178345
Min Value: -4.6142706871032715
Max Value: 25.219873428344727
Min Value: -1.6715457439422607
Value: true
Axis: Z
Channel Name: intensity
Expand All @@ -74,6 +74,7 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /lio_sam/mapping/cloud_registered
Expand All @@ -91,14 +92,16 @@ Visualization Manager:
Value: true
imu_link:
Value: true
laser_sensor_frame:
Value: true
lidar_link:
Value: true
map:
Value: true
navsat_link:
Value: true
odom:
Value: true
os0_32/os_sensor:
Value: true
Marker Scale: 1
Name: TF
Show Arrows: true
Expand All @@ -107,7 +110,15 @@ Visualization Manager:
Tree:
map:
odom:
{}
base_link:
chassis_link:
imu_link:
laser_sensor_frame:
{}
navsat_link:
{}
lidar_link:
{}
Update Interval: 0
Value: true
- Alpha: 1
Expand All @@ -133,6 +144,7 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /lio_sam/mapping/path
Expand All @@ -141,7 +153,8 @@ Visualization Manager:
Enabled: true
Name: MarkerArray
Namespaces:
{}
loop_edges: true
loop_nodes: true
Topic:
Depth: 5
Durability Policy: Volatile
Expand Down Expand Up @@ -176,12 +189,41 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: System Default
Value: /points
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz_default_plugins/Path
Color: 25; 255; 0
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Lines
Line Width: 0.029999999329447746
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /gps_path
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Expand All @@ -197,6 +239,9 @@ Visualization Manager:
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Covariance x: 0.25
Covariance y: 0.25
Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
Expand Down Expand Up @@ -225,7 +270,7 @@ Visualization Manager:
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 142.68260192871094
Distance: 10
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Expand All @@ -235,29 +280,29 @@ Visualization Manager:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.47039806842803955
Pitch: 0.7853981852531433
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.5903979539871216
Yaw: 0.7853981852531433
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1016
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000020a0000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004130000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd00000004000000000000020a0000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004110000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1848
X: 72
Width: 1846
X: 74
Y: 27
Loading