EmbodiedAI
/
quadruped_ros2_control
mirror of https://github.com/legubiao/quadruped_ros2_control.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

cartographer and gazebo test

This commit is contained in:
Huang Zhenbiao 2025-03-24 01:01:36 +08:00
parent dbfb081486
commit ff2d9e1928
10 changed files with 1258 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp
View File

@ -39,7 +39,7 @@ namespace ocs2::legged_robot
void TargetManager::update(SystemObservation& observation) void TargetManager::update(SystemObservation& observation)
{ {
vector_t cmdGoal = vector_t::Zero(6); vector_t cmdGoal = vector_t::Zero(6);
if (buffer_.readFromRT() == nullptr) if (buffer_.readFromRT() == nullptr || twist_count <= 0)
{ {
cmdGoal[0] = ctrl_component_.control_inputs_.ly * target_displacement_velocity_; cmdGoal[0] = ctrl_component_.control_inputs_.ly * target_displacement_velocity_;
cmdGoal[1] = -ctrl_component_.control_inputs_.lx * target_displacement_velocity_; cmdGoal[1] = -ctrl_component_.control_inputs_.lx * target_displacement_velocity_;

1
descriptions/deep_robotics/lite3_description/config/gazebo.yaml
View File

@ -23,6 +23,7 @@ imu_sensor_broadcaster:
ros__parameters: ros__parameters:
sensor_name: "imu_sensor" sensor_name: "imu_sensor"
frame_id: "imu_link" frame_id: "imu_link"
update_rate: 200
ocs2_quadruped_controller: ocs2_quadruped_controller:
ros__parameters: ros__parameters:

1
descriptions/deep_robotics/lite3_description/config/robot_control.yaml
View File

@ -24,7 +24,6 @@ imu_sensor_broadcaster:
sensor_name: "imu_sensor" sensor_name: "imu_sensor"
frame_id: "imu_link" frame_id: "imu_link"
unitree_guide_controller: unitree_guide_controller:
ros__parameters: ros__parameters:
update_rate: 200 # Hz update_rate: 200 # Hz

16
libraries/gz_quadruped_playground/README.md
View File

@ -34,6 +34,22 @@ colcon build --packages-up-to gz_quadruped_playground --symlink-install
ros2 launch gz_quadruped_playground gazebo.launch.py controller:=ocs2 world:=warehouse ros2 launch gz_quadruped_playground gazebo.launch.py controller:=ocs2 world:=warehouse
``` ```
## SLAM Test
### Record Rosbag
```bash
cd ~/ros2_ws
ros2 bag record /rgbd_d435/points /rgbd_d435/depth_image /scan/points /imu_sensor_broadcaster/imu /odom /tf /tf_static /joint_states
```
### Fast LIO
```bash
source ~/ros2_ws/install/setup.bash
ros2 launch gz_quadruped_playground fast_lio.launch.py
```
## Related Materials ## Related Materials
* [Gazebo OdometryPublisher Plugin](https://gazebosim.org/api/sim/8/classgz_1_1sim_1_1systems_1_1OdometryPublisher.html#details) * [Gazebo OdometryPublisher Plugin](https://gazebosim.org/api/sim/8/classgz_1_1sim_1_1systems_1_1OdometryPublisher.html#details)

44
libraries/gz_quadruped_playground/config/cartographer/cartographer.lua Normal file
View File

@ -0,0 +1,44 @@
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
tracking_frame = "base",
published_frame = "odom",
odom_frame = "odom",
provide_odom_frame = false,
publish_frame_projected_to_2d = false,
use_odometry = true,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 0,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 1,
lookup_transform_timeout_sec = 0.1,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 0.5,
fixed_frame_pose_sampling_ratio = 0.9,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 160
TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1
MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 7
POSE_GRAPH.optimization_problem.huber_scale = 5e2
POSE_GRAPH.optimize_every_n_nodes = 100
POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10
POSE_GRAPH.constraint_builder.min_score = 0.62
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66
return options

565
libraries/gz_quadruped_playground/config/cartographer/cartographer.rviz Normal file
View File

@ -0,0 +1,565 @@
Panels:
- Class: rviz_common/Displays
Help Height: 138
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /TF1
- /Map1/Status1
- /Cartographer1
- /PointCloud21
- /PointCloud21/Autocompute Value Bounds1
Splitter Ratio: 0.3916349709033966
Tree Height: 1754
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /Publish Point1
- /2D Pose Estimate1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz_default_plugins/TF
Enabled: true
Filter (blacklist): ""
Filter (whitelist): ""
Frame Timeout: 15
Frames:
All Enabled: true
FL_FOOT:
Value: true
FL_HIP:
Value: true
FL_SHANK:
Value: true
FL_THIGH:
Value: true
FR_FOOT:
Value: true
FR_HIP:
Value: true
FR_SHANK:
Value: true
FR_THIGH:
Value: true
HL_FOOT:
Value: true
HL_HIP:
Value: true
HL_SHANK:
Value: true
HL_THIGH:
Value: true
HR_FOOT:
Value: true
HR_HIP:
Value: true
HR_SHANK:
Value: true
HR_THIGH:
Value: true
TORSO:
Value: true
base:
Value: true
camera_d435:
Value: true
imu_link:
Value: true
lidar_lslidar:
Value: true
odom:
Value: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
odom:
base:
TORSO:
FL_HIP:
FL_THIGH:
FL_SHANK:
FL_FOOT:
{}
FR_HIP:
FR_THIGH:
FR_SHANK:
FR_FOOT:
{}
HL_HIP:
HL_THIGH:
HL_SHANK:
HL_FOOT:
{}
HR_HIP:
HR_THIGH:
HR_SHANK:
HR_FOOT:
{}
imu_link:
{}
camera_d435:
{}
lidar_lslidar:
{}
Update Interval: 0
Value: true
- Angle Tolerance: 0.10000000149011612
Class: rviz_default_plugins/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: false
Enabled: false
Keep: 100
Name: Odometry
Position Tolerance: 0.10000000149011612
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Color: 255; 25; 0
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Value: Arrow
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /odom
Value: false
- Alpha: 0.699999988079071
Binary representation: false
Binary threshold: 100
Class: rviz_default_plugins/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /map
Update Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /map_updates
Use Timestamp: false
Value: true
- Class: rviz_common/Group
Displays:
- Class: rviz_common/Group
Displays:
- Alpha: 0.699999988079071
Binary representation: false
Binary threshold: 100
Class: rviz_default_plugins/Map
Color Scheme: costmap
Draw Behind: true
Enabled: true
Name: Map
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /global_costmap/costmap
Update Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /global_costmap/costmap_updates
Use Timestamp: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz_default_plugins/Path
Color: 255; 0; 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: /global_plan
Value: true
Enabled: true
Name: Global Map
- Class: rviz_common/Group
Displays:
- Alpha: 1
Class: rviz_default_plugins/Polygon
Color: 25; 255; 0
Enabled: true
Name: Polygon
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /local_costmap/footprint
Value: true
- Alpha: 0.699999988079071
Binary representation: false
Binary threshold: 100
Class: rviz_default_plugins/Map
Color Scheme: costmap
Draw Behind: false
Enabled: true
Name: Map
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /local_costmap/costmap
Update Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /local_costmap/costmap_updates
Use Timestamp: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz_default_plugins/Path
Color: 255; 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: /local_plan
Value: true
Enabled: true
Name: Local Map
- Alpha: 1
Arrow Length: 0.05000000074505806
Axes Length: 0.30000001192092896
Axes Radius: 0.009999999776482582
Class: rviz_default_plugins/PoseArray
Color: 0; 192; 0
Enabled: true
Head Length: 0.07000000029802322
Head Radius: 0.029999999329447746
Name: PoseArray
Shaft Length: 0.23000000417232513
Shaft Radius: 0.009999999776482582
Shape: Arrow (Flat)
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /particlecloud
Value: true
Enabled: false
Name: Navigation
- Class: rviz_common/Group
Displays:
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.18203988671302795
Min Value: 0.18195410072803497
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 0; 255; 0
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: scan_matched_points2
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Boxes
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /scan_matched_points2
Use Fixed Frame: true
Use rainbow: true
Value: true
- Class: rviz_default_plugins/MarkerArray
Enabled: true
Name: Trajectories
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /trajectory_node_list
Value: true
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Name: Constraints
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /constraint_list
Value: false
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Name: Landmark Poses
Namespaces:
{}
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /landmark_poses_list
Value: false
Enabled: true
Name: Cartographer
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 4.224797248840332
Min Value: -0.040010638535022736
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: AxisColor
Decay Time: 0
Enabled: true
Invert Rainbow: true
Max Color: 255; 255; 255
Max Intensity: 0
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.004999999888241291
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /scan/points
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /rgbd_d435/points
Use Fixed Frame: true
Use rainbow: true
Value: false
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: odom
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /move_base_simple/goal
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
- Class: rviz_default_plugins/SetInitialPose
Covariance x: 0.25
Covariance y: 0.25
Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: initialpose
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 18.017410278320312
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 1.4430718421936035
Y: -0.9982128739356995
Z: 1.1156216859817505
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.9897968769073486
Target Frame: <Fixed Frame>
Value: Orbit (rviz_default_plugins)
Yaw: 3.1460328102111816
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 2166
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd0000000400000000000002f0000007d6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000070000007d60000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015d000007d6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000070000007d60000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000b17000007d600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 3964
X: 132
Y: 64

411
libraries/gz_quadruped_playground/config/fast_lio/fastlio.rviz Normal file
View File

@ -0,0 +1,411 @@
Panels:
- Class: rviz_common/Displays
Help Height: 138
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /TF1
- /Path1
- /CloudRegistered1
- /RobotModel1
- /PointCloud21
Splitter Ratio: 0.5
Tree Height: 1075
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz_common/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: CloudRegistered
Visualization Manager:
Class: ""
Displays:
- Class: rviz_default_plugins/TF
Enabled: true
Filter (blacklist): ""
Filter (whitelist): ""
Frame Timeout: 15
Frames:
All Enabled: true
FL_FOOT:
Value: true
FL_HIP:
Value: true
FL_SHANK:
Value: true
FL_THIGH:
Value: true
FR_FOOT:
Value: true
FR_HIP:
Value: true
FR_SHANK:
Value: true
FR_THIGH:
Value: true
HL_FOOT:
Value: true
HL_HIP:
Value: true
HL_SHANK:
Value: true
HL_THIGH:
Value: true
HR_FOOT:
Value: true
HR_HIP:
Value: true
HR_SHANK:
Value: true
HR_THIGH:
Value: true
TORSO:
Value: true
base:
Value: true
body:
Value: true
camera_d435:
Value: true
camera_init:
Value: true
imu_link:
Value: true
lidar_lslidar:
Value: true
odom:
Value: true
Marker Scale: 5
Name: TF
Show Arrows: true
Show Axes: true
Show Names: false
Tree:
camera_init:
body:
{}
Update Interval: 0
Value: true
- Angle Tolerance: 0.10000000149011612
Class: rviz_default_plugins/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: false
Keep: 100
Name: Odometry
Position Tolerance: 0.10000000149011612
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Color: 255; 25; 0
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Value: Arrow
Topic:
Depth: 5
Durability Policy: Volatile
Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /Odometry
Value: false
- 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: /path
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 2.8755857944488525
Min Value: -0.3105084300041199
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: AxisColor
Decay Time: 1200
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 186
Min Color: 0; 0; 0
Min Intensity: 0
Name: CloudRegistered
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /cloud_registered
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 184
Min Color: 0; 0; 0
Min Intensity: 0
Name: CloudEffected
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /cloud_effected
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: -9999
Min Value: 9999
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: AxisColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 255
Min Color: 0; 0; 0
Min Intensity: 0
Name: CloudMap
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.05000000074505806
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /Laser_map
Use Fixed Frame: true
Use rainbow: true
Value: true
- Alpha: 1
Class: rviz_default_plugins/RobotModel
Collision Enabled: false
Description File: ""
Description Source: Topic
Description Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /robot_description
Enabled: false
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
Mass Properties:
Inertia: false
Mass: false
Name: RobotModel
TF Prefix: ""
Update Interval: 0
Value: false
Visual Enabled: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz_default_plugins/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 190
Min Color: 0; 0; 0
Min Intensity: 0
Name: PointCloud2
Position Transformer: XYZ
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /cx/lslidar_point_cloud
Use Fixed Frame: true
Use rainbow: true
Value: false
Enabled: true
Global Options:
Background Color: 0; 0; 0
Fixed Frame: camera_init
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- 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
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 6.59425163269043
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 12.15172004699707
Y: 2.1967546939849854
Z: 8.923361778259277
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.48979365825653076
Target Frame: <Fixed Frame>
Value: Orbit (rviz_default_plugins)
Yaw: 0.1522158682346344
Saved: ~
Window Geometry:
Displays:
collapsed: true
Height: 1593
Hide Left Dock: true
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd0000000400000000000003400000052ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000700000052f0000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015d0000052ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000700000052f0000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009d70000005efc0100000002fb0000000800540069006d00650100000000000009d70000047a00fffffffb0000000800540069006d006501000000000000045000000000000000000000086e0000052f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 2519
X: 1348
Y: 187

51
libraries/gz_quadruped_playground/config/fast_lio/fastlio.yaml Normal file
View File

@ -0,0 +1,51 @@
/**:
ros__parameters:
feature_extract_enable: false
point_filter_num: 4
max_iteration: 3
filter_size_surf: 0.5
filter_size_map: 0.5
cube_side_length: 1000.0
runtime_pos_log_enable: false
map_file_path: "./test.pcd"
common:
# lid_topic: "/rgbd_d435/points"
lid_topic: "/scan/points"
imu_topic: "/imu_sensor_broadcaster/imu"
# lid_topic: "/cx/lslidar_point_cloud"
# imu_topic: "/imu/data"
time_sync_en: false # ONLY turn on when external time synchronization is really not possible
time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
# This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
preprocess:
lidar_type: 5 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR,
scan_line: 32
scan_rate: 10 # only need to be set for velodyne, unit: Hz,
timestamp_unit: 2 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
blind: 2.0
mapping:
acc_cov: 0.1
gyr_cov: 0.1
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
fov_degree: 360.0
det_range: 100.0
extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic,
extrinsic_T: [ 0., 0., 0.28 ]
extrinsic_R: [ 1., 0., 0.,
0., 1., 0.,
0., 0., 1. ]
publish:
path_en: true # false: close the path output
scan_publish_en: true # false: close all the point cloud output
dense_publish_en: true # false: low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame
pcd_save:
pcd_save_en: false
interval: -1 # how many LiDAR frames saved in each pcd file;
# -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

96
libraries/gz_quadruped_playground/launch/slam/cartographer.launch.py Normal file
View File

@ -0,0 +1,96 @@
# Copyright 2019 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# Author: Darby Lim
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch.substitutions import ThisLaunchFileDir
from launch_ros.actions import Node
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time', default='true')
prefix = get_package_share_directory('gz_quadruped_playground')
cartographer_config_dir = LaunchConfiguration('cartographer_config_dir', default=os.path.join(
prefix, 'config', 'cartographer'))
configuration_basename = LaunchConfiguration('configuration_basename',
default='cartographer.lua')
resolution = LaunchConfiguration('resolution', default='0.05')
publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0')
rviz_config_dir = os.path.join(prefix, 'config', 'cartographer', 'cartographer.rviz')
return LaunchDescription([
DeclareLaunchArgument(
'cartographer_config_dir',
default_value=cartographer_config_dir,
description='Full path to config file to load'),
DeclareLaunchArgument(
'configuration_basename',
default_value=configuration_basename,
description='Name of lua file for cartographer'),
DeclareLaunchArgument(
'use_sim_time',
default_value='true',
description='Use simulation (Gazebo) clock if true'),
Node(
package='cartographer_ros',
executable='cartographer_node',
name='cartographer_node',
output='screen',
parameters=[{'use_sim_time': use_sim_time}],
arguments=['-configuration_directory', cartographer_config_dir,
'-configuration_basename', configuration_basename],
remappings=[
('/points2', '/scan/points'),
# ('/points2_2', '/rgbd_d435/points'),
('/imu', '/imu_sensor_broadcaster/imu'),
]
),
DeclareLaunchArgument(
'resolution',
default_value=resolution,
description='Resolution of a grid cell in the published occupancy grid'),
DeclareLaunchArgument(
'publish_period_sec',
default_value=publish_period_sec,
description='OccupancyGrid publishing period'),
Node(
package='cartographer_ros',
executable='cartographer_occupancy_grid_node',
name='cartographer_occupancy_grid_node',
output='screen',
parameters=[{'use_sim_time': use_sim_time}],
arguments=['-resolution', resolution, '-publish_period_sec', publish_period_sec]),
Node(
package='rviz2',
executable='rviz2',
name='rviz2',
arguments=['-d', rviz_config_dir],
parameters=[{'use_sim_time': use_sim_time}]
),
])

73
libraries/gz_quadruped_playground/launch/slam/fast_lio.launch.py Normal file
View File

@ -0,0 +1,73 @@
import os.path
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch.conditions import IfCondition
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch.substitutions import ThisLaunchFileDir
def generate_launch_description():
package_path = get_package_share_directory('gz_quadruped_playground')
default_config_path = os.path.join(package_path, 'config', 'fast_lio')
default_rviz_config_path = os.path.join(
package_path, 'config', 'fast_lio', 'fastlio.rviz')
use_sim_time = LaunchConfiguration('use_sim_time')
config_path = LaunchConfiguration('config_path')
config_file = LaunchConfiguration('config_file')
rviz_use = LaunchConfiguration('rviz')
rviz_cfg = LaunchConfiguration('rviz_cfg')
declare_use_sim_time_cmd = DeclareLaunchArgument(
'use_sim_time', default_value='false',
description='Use simulation (Gazebo) clock if true'
)
declare_config_path_cmd = DeclareLaunchArgument(
'config_path', default_value=default_config_path,
description='Yaml config file path'
)
decalre_config_file_cmd = DeclareLaunchArgument(
'config_file', default_value='fastlio.yaml',
description='Config file'
)
declare_rviz_cmd = DeclareLaunchArgument(
'rviz', default_value='true',
description='Use RViz to monitor results'
)
declare_rviz_config_path_cmd = DeclareLaunchArgument(
'rviz_cfg', default_value=default_rviz_config_path,
description='RViz config file path'
)
fast_lio_node = Node(
package='fast_lio',
executable='fastlio_mapping',
parameters=[PathJoinSubstitution([config_path, config_file]),
{'use_sim_time': use_sim_time, 'tf_prefix': 'base'}],
output='screen'
)
rviz_node = Node(
package='rviz2',
executable='rviz2',
arguments=['-d', rviz_cfg],
condition=IfCondition(rviz_use)
)
ld = LaunchDescription()
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_config_path_cmd)
ld.add_action(decalre_config_file_cmd)
ld.add_action(declare_rviz_cmd)
ld.add_action(declare_rviz_config_path_cmd)
ld.add_action(fast_lio_node)
ld.add_action(rviz_node)
return ld