feat(slam): add rtabmap_ros
This commit is contained in:
X-lanni
@@ -0,0 +1,10 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(rtabmap_demos)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(DIRECTORY launch config params data
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,85 @@
|
||||
# rtabmap_demos
|
||||
+ [Outdoor Stereo VSLAM](#outdoor-stereo-vslam)
|
||||
+ [Indoor 2D LiDAR and RGB-D SLAM](#indoor-2d-lidar-and-rgb-d-slam)
|
||||
+ [Multi-Session Indoor 2D LiDAR and RGB-D SLAM](#multi-session-indoor-2d-lidar-and-rgb-d-slam)
|
||||
+ [Find-Object with SLAM](#find-object-with-slam)
|
||||
+ [Turtlebot4 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot4-nav2-2d-lidar-and-rgb-d-slam)
|
||||
+ [Turtlebot3 Nav2 and 2D LiDAR SLAM](#turtlebot3-nav2-and-2d-lidar-slam)
|
||||
+ [Turtlebot3 Nav2 and RGB-D SLAM](#turtlebot3-nav2-and-rgb-d-slam)
|
||||
+ [Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2-2d-lidar-and-rgb-d-slam)
|
||||
+ [Turtlebot3 Nav2, Fake 2D LiDAR and RGB-D SLAM](#turtlebot3-nav2-fake-2d-lidar-and-rgb-d-slam)
|
||||
+ [Champ Quadruped Nav2, Elevation Map and VSLAM](#champ-quadruped-nav2-elevation-map-and-vslam)
|
||||
+ [Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM](#clearpath-husky-nav2-2d-lidar-and-rgb-d-slam)
|
||||
+ [Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM](#clearpath-husky-nav2-3d-lidar-and-rgb-d-slam)
|
||||
+ [Clearpath Husky Nav2, 3D LiDAR Assembling and RGB-D SLAM](#clearpath-husky-nav2-3d-lidar-assembling-and-rgb-d-slam)
|
||||
+ [Isaac Sim Nav2 and Stereo SLAM](#isaac-sim-nav2-and-stereo-slam)
|
||||
+ [Isaac Sim Nav2 and RGB-D VSLAM](#isaac-sim-nav2-and-rgb-d-vslam)
|
||||
|
||||
### Outdoor Stereo VSLAM
|
||||
[stereo_outdoor_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/stereo_outdoor_demo.launch.py) ([Video](https://youtu.be/qpTS7kg9J3A))
|
||||
|
||||
|
||||
|
||||
### Indoor 2D LiDAR and RGB-D SLAM
|
||||
[robot_mapping_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/robot_mapping_demo.launch.py) (Videos: [rtabmap_viz](https://youtu.be/c0qrEd5rR7M), [rviz](https://youtu.be/MQoSDpAsqps))
|
||||
|
||||
|
||||
|
||||
### Multi-Session Indoor 2D LiDAR and RGB-D SLAM
|
||||
[multisession_mapping_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/multisession_mapping_demo.launch.py) ([Video](https://youtu.be/XrnyhaxPCro))
|
||||
|
||||
|
||||
|
||||
### Find-Object with SLAM
|
||||
[find_object_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/find_object_demo.launch.py) ([Video](https://youtu.be/o1GSQanY-Do))
|
||||
|
||||
|
||||
|
||||
### Turtlebot4 Nav2, 2D LiDAR and RGB-D SLAM
|
||||
[turtlebot4_sim_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot4/turtlebot4_sim_demo.launch.py)
|
||||
|
||||
|
||||
### Turtlebot3 Nav2 and 2D LiDAR SLAM
|
||||
[turtlebot3_sim_scan_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_scan_demo.launch.py)
|
||||
|
||||
|
||||
### Turtlebot3 Nav2 and RGB-D SLAM
|
||||
[turtlebot3_sim_rgbd_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_demo.launch.py)
|
||||
|
||||
|
||||
### Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM
|
||||
[turtlebot3_sim_rgbd_scan_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_scan_demo.launch.py)
|
||||
|
||||
|
||||
### Turtlebot3 Nav2, Fake 2D LiDAR and RGB-D SLAM
|
||||
[turtlebot3_sim_rgbd_fake_scan_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/turtlebot3/turtlebot3_sim_rgbd_fake_scan_demo.launch.py)
|
||||
|
||||
* Red: Scan generated from camera's depth.
|
||||
* Orange: Locally assembled scans used for proximity detection.
|
||||
* Yellow: The map.
|
||||
|
||||
|
||||
### Champ Quadruped Nav2, Elevation Map and VSLAM
|
||||
[champ_sim_vslam.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/champ/champ_sim_vslam.launch.py)
|
||||
|
||||
|
||||
### Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM
|
||||
[husky_sim_scan2d_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/husky/husky_sim_scan2d_demo.launch.py)
|
||||
|
||||
|
||||
### Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM
|
||||
[husky_sim_scan3d_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/husky/husky_sim_scan3d_demo.launch.py)
|
||||
|
||||
|
||||
### Clearpath Husky Nav2, 3D LiDAR Assembling and RGB-D SLAM
|
||||
[husky_sim_scan3d_assemble_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/husky/husky_sim_scan3d_assemble_demo.launch.py)
|
||||
|
||||
|
||||
### Isaac Sim Nav2 and Stereo SLAM
|
||||
[isaac_sim_vslam_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/isaac/isaac_sim_vslam_demo.launch.py)
|
||||
|
||||
|
||||
### Isaac Sim Nav2 and RGB-D VSLAM
|
||||
[isaac_sim_vslam_demo.launch.py](https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_demos/launch/isaac/isaac_sim_vslam_demo.launch.py) stereo:=false vo:=rtabmap
|
||||
|
||||
|
||||
@@ -0,0 +1,496 @@
|
||||
Panels:
|
||||
- Class: rviz_common/Displays
|
||||
Help Height: 0
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 355
|
||||
- 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: MapCloud
|
||||
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
|
||||
- Alpha: 0.699999988079071
|
||||
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_default_plugins/TF
|
||||
Enabled: true
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: true
|
||||
base_footprint:
|
||||
Value: true
|
||||
base_link:
|
||||
Value: true
|
||||
camera_accel_frame:
|
||||
Value: true
|
||||
camera_accel_optical_frame:
|
||||
Value: true
|
||||
camera_color_frame:
|
||||
Value: true
|
||||
camera_color_optical_frame:
|
||||
Value: true
|
||||
camera_depth_frame:
|
||||
Value: true
|
||||
camera_depth_optical_frame:
|
||||
Value: true
|
||||
camera_gyro_frame:
|
||||
Value: true
|
||||
camera_gyro_optical_frame:
|
||||
Value: true
|
||||
camera_ir_frame:
|
||||
Value: true
|
||||
camera_ir_optical_frame:
|
||||
Value: true
|
||||
camera_link:
|
||||
Value: true
|
||||
laser_link:
|
||||
Value: true
|
||||
left_front_wheel_link:
|
||||
Value: true
|
||||
left_rear_wheel_link:
|
||||
Value: true
|
||||
map:
|
||||
Value: true
|
||||
odom:
|
||||
Value: true
|
||||
right_front_wheel_link:
|
||||
Value: true
|
||||
right_rear_wheel_link:
|
||||
Value: true
|
||||
Marker Scale: 1
|
||||
Name: TF
|
||||
Show Arrows: true
|
||||
Show Axes: true
|
||||
Show Names: false
|
||||
Tree:
|
||||
map:
|
||||
odom:
|
||||
base_footprint:
|
||||
base_link:
|
||||
camera_link:
|
||||
camera_depth_frame:
|
||||
camera_accel_frame:
|
||||
camera_accel_optical_frame:
|
||||
{}
|
||||
camera_color_frame:
|
||||
camera_color_optical_frame:
|
||||
{}
|
||||
camera_depth_optical_frame:
|
||||
{}
|
||||
camera_gyro_frame:
|
||||
camera_gyro_optical_frame:
|
||||
{}
|
||||
camera_ir_frame:
|
||||
camera_ir_optical_frame:
|
||||
{}
|
||||
laser_link:
|
||||
{}
|
||||
left_front_wheel_link:
|
||||
{}
|
||||
left_rear_wheel_link:
|
||||
{}
|
||||
right_front_wheel_link:
|
||||
{}
|
||||
right_rear_wheel_link:
|
||||
{}
|
||||
Update Interval: 0
|
||||
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/LaserScan
|
||||
Color: 237; 51; 59
|
||||
Color Transformer: Intensity
|
||||
Decay Time: 0
|
||||
Enabled: true
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 231
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 6
|
||||
Name: LaserScan
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /scan
|
||||
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: rtabmap_rviz_plugins/MapCloud
|
||||
Cloud decimation: 4
|
||||
Cloud from scan: false
|
||||
Cloud max depth (m): 4
|
||||
Cloud min depth (m): 0
|
||||
Cloud voxel size (m): 0.009999999776482582
|
||||
Color: 255; 255; 255
|
||||
Color Transformer: RGB8
|
||||
Download graph: false
|
||||
Download map: false
|
||||
Download namespace: rtabmap
|
||||
Enabled: true
|
||||
Filter ceiling (m): 0
|
||||
Filter floor (m): 0
|
||||
Invert Rainbow: false
|
||||
Max Color: 255; 255; 255
|
||||
Max Intensity: 4096
|
||||
Min Color: 0; 0; 0
|
||||
Min Intensity: 0
|
||||
Name: MapCloud
|
||||
Node filtering angle (degrees): 30
|
||||
Node filtering radius (m): 0
|
||||
Position Transformer: XYZ
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /mapData
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rtabmap_rviz_plugins/MapGraph
|
||||
Enabled: true
|
||||
Global loop closure: 255; 0; 0
|
||||
Landmark: 0; 128; 0
|
||||
Local loop closure: 255; 255; 0
|
||||
Merged neighbor: 255; 170; 0
|
||||
Name: MapGraph
|
||||
Neighbor: 0; 0; 255
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /mapGraph
|
||||
User: 255; 0; 0
|
||||
Value: true
|
||||
Virtual: 255; 0; 255
|
||||
- Class: rviz_common/Group
|
||||
Displays:
|
||||
- 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: 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: Current Frame
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odom_last_frame
|
||||
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: 0; 255; 0
|
||||
Color Transformer: RGB8
|
||||
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: Local Feature Map
|
||||
Position Transformer: XYZ
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Style: Points
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
Filter size: 10
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /odom_local_map
|
||||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
Enabled: true
|
||||
Name: VO
|
||||
- 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: true
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
base_footprint:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
base_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
laser_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
left_front_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
left_rear_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_front_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
right_rear_wheel_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
Mass Properties:
|
||||
Inertia: false
|
||||
Mass: false
|
||||
Name: RobotModel
|
||||
TF Prefix: ""
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
- Class: rviz_default_plugins/Image
|
||||
Enabled: true
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image
|
||||
Normalize Range: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /camera/color/image_raw
|
||||
Value: true
|
||||
- Class: rviz_default_plugins/Image
|
||||
Enabled: true
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image
|
||||
Normalize Range: true
|
||||
Topic:
|
||||
Depth: 5
|
||||
Durability Policy: Volatile
|
||||
History Policy: Keep Last
|
||||
Reliability Policy: Reliable
|
||||
Value: /camera/depth/image_raw
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: map
|
||||
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: 3.969904899597168
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0.30078303813934326
|
||||
Y: -0.3047744929790497
|
||||
Z: -2.0372681319713593e-08
|
||||
Focal Shape Fixed Size: false
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.7953978776931763
|
||||
Target Frame: base_footprint
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 3.9203996658325195
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 921
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: true
|
||||
Image:
|
||||
collapsed: false
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000156000002fbfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001a0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001e30000008e0000002800fffffffb0000000a0049006d0061006700650100000277000000c10000002800ffffff000000010000010f000002fbfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002fb000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006590000003efc0100000002fb0000000800540069006d0065010000000000000659000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004fd000002fb00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: true
|
||||
Width: 1625
|
||||
X: 176
|
||||
Y: 77
|
||||
@@ -0,0 +1,188 @@
|
||||
[General]
|
||||
windowGeometry=@ByteArray(\x1\xd9\xd0\xcb\0\x3\0\0\0\0\x4\x34\0\0\x1\x8e\0\0\x6\x9a\0\0\x4\x5\0\0\x4\x34\0\0\x1\xb3\0\0\x6\x9a\0\0\x4\x5\0\0\0\0\0\0\0\0\a\x80\0\0\x4\x34\0\0\x1\xb3\0\0\x6\x9a\0\0\x4\x5)
|
||||
windowState=@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\0\xda\0\0\x2'\xfc\x2\0\0\0\x1\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0o\0\x62\0j\0\x65\0\x63\0t\0s\x1\0\0\0\x16\0\0\x2'\0\0\0\xc4\0\xff\xff\xff\0\0\0\x1\0\0\x1h\0\0\x1\x86\xfc\x2\0\0\0\x2\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0\x61\0r\0\x61\0m\0\x65\0t\0\x65\0r\0s\0\0\0\0\x16\0\0\x1\x86\0\0\0\xa8\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0i\0s\0t\0i\0\x63\0s\0\0\0\0\0\xff\xff\xff\xff\0\0\x1\x35\0\xff\xff\xff\0\0\0\x3\0\0\0\0\0\0\0\0\xfc\x1\0\0\0\x1\xfb\0\0\0\x1e\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0l\0o\0t\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\0\0\x1\x87\0\0\x2'\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\0)
|
||||
|
||||
[Camera]
|
||||
1deviceId=0
|
||||
2imageWidth=640
|
||||
3imageHeight=480
|
||||
4imageRate=0
|
||||
5mediaPath=
|
||||
6useTcpCamera=false
|
||||
7IP=127.0.0.1
|
||||
8port=5000
|
||||
9queueSize=1
|
||||
|
||||
[Feature2D]
|
||||
1Detector="5:Dense;Fast;GFTT;MSER;ORB;SIFT;Star;SURF;BRISK;AGAST;KAZE;AKAZE;SuperPointTorch"
|
||||
2Descriptor="2:Brief;ORB;SIFT;SURF;BRISK;FREAK;KAZE;AKAZE;LUCID;LATCH;DAISY;SuperPointTorch"
|
||||
3MaxFeatures=0
|
||||
4Affine=false
|
||||
5AffineCount=6
|
||||
6SubPix=false
|
||||
7SubPixWinSize=3
|
||||
8SubPixIterations=30
|
||||
9SubPixEps=0.02
|
||||
AGAST_nonmaxSuppression=true
|
||||
AGAST_threshold=10
|
||||
AKAZE_descriptorChannels=3
|
||||
AKAZE_descriptorSize=0
|
||||
AKAZE_nOctaveLayers=4
|
||||
AKAZE_nOctaves=4
|
||||
AKAZE_threshold=0.001
|
||||
BRISK_octaves=3
|
||||
BRISK_patternScale=1
|
||||
BRISK_thresh=30
|
||||
Brief_bytes=32
|
||||
DAISY_interpolation=true
|
||||
DAISY_q_hist=8
|
||||
DAISY_q_radius=3
|
||||
DAISY_q_theta=8
|
||||
DAISY_radius=15
|
||||
DAISY_use_orientation=false
|
||||
Dense_featureScaleLevels=1
|
||||
Dense_featureScaleMul=0.1
|
||||
Dense_initFeatureScale=1
|
||||
Dense_initImgBound=0
|
||||
Dense_initXyStep=6
|
||||
Dense_varyImgBoundWithScale=false
|
||||
Dense_varyXyStepWithScale=true
|
||||
FREAK_nOctaves=4
|
||||
FREAK_orientationNormalized=true
|
||||
FREAK_patternScale=22
|
||||
FREAK_scaleNormalized=true
|
||||
Fast_gpu=false
|
||||
Fast_keypointsRatio=0.05
|
||||
Fast_maxNpoints=5000
|
||||
Fast_nonmaxSuppression=true
|
||||
Fast_threshold=10
|
||||
GFTT_blockSize=3
|
||||
GFTT_k=0.04
|
||||
GFTT_maxCorners=1000
|
||||
GFTT_minDistance=1
|
||||
GFTT_qualityLevel=0.01
|
||||
GFTT_useHarrisDetector=false
|
||||
KAZE_extended=false
|
||||
KAZE_nOctaveLayers=4
|
||||
KAZE_nOctaves=4
|
||||
KAZE_threshold=0.001
|
||||
KAZE_upright=false
|
||||
LATCH_bytes=32
|
||||
LATCH_half_ssd_size=3
|
||||
LATCH_rotationInvariance=true
|
||||
LUCID_blur_kernel=2
|
||||
LUCID_kernel=1
|
||||
MSER_areaThreshold=1.01
|
||||
MSER_delta=5
|
||||
MSER_edgeBlurSize=5
|
||||
MSER_maxArea=14400
|
||||
MSER_maxEvolution=200
|
||||
MSER_maxVariation=0.25
|
||||
MSER_minArea=60
|
||||
MSER_minDiversity=0.2
|
||||
MSER_minMargin=0.003
|
||||
ORB_WTA_K=2
|
||||
ORB_blurForDescriptor=false
|
||||
ORB_edgeThreshold=31
|
||||
ORB_firstLevel=0
|
||||
ORB_gpu=false
|
||||
ORB_nFeatures=500
|
||||
ORB_nLevels=8
|
||||
ORB_patchSize=31
|
||||
ORB_scaleFactor=1.2
|
||||
ORB_scoreType=0
|
||||
SIFT_contrastThreshold=0.04
|
||||
SIFT_edgeThreshold=10
|
||||
SIFT_nOctaveLayers=3
|
||||
SIFT_nfeatures=0
|
||||
SIFT_rootSIFT=false
|
||||
SIFT_sigma=1.6
|
||||
SURF_extended=true
|
||||
SURF_gpu=false
|
||||
SURF_hessianThreshold=600
|
||||
SURF_keypointsRatio=0.01
|
||||
SURF_nOctaveLayers=2
|
||||
SURF_nOctaves=4
|
||||
SURF_upright=false
|
||||
Star_lineThresholdBinarized=8
|
||||
Star_lineThresholdProjected=10
|
||||
Star_maxSize=45
|
||||
Star_responseThreshold=30
|
||||
Star_suppressNonmaxSize=5
|
||||
SuperPointTorch_NMS=true
|
||||
SuperPointTorch_NMS_radius=4
|
||||
SuperPointTorch_cuda=false
|
||||
SuperPointTorch_modelPath=
|
||||
SuperPointTorch_threshold=0.2
|
||||
|
||||
[%General]
|
||||
autoPauseOnDetection=false
|
||||
autoScreenshotPath=
|
||||
autoScroll=true
|
||||
autoStartCamera=false
|
||||
autoUpdateObjects=true
|
||||
controlsShown=false
|
||||
debug=false
|
||||
imageFormats=*.png *.jpg *.bmp *.tiff *.ppm
|
||||
invertedSearch=true
|
||||
mirrorView=false
|
||||
multiDetection=false
|
||||
multiDetectionRadius=30
|
||||
nextObjID=9
|
||||
port=0
|
||||
sendNoObjDetectedEvents=false
|
||||
threads=1
|
||||
videoFormats=*.avi *.m4v *.mp4
|
||||
vocabularyFixed=false
|
||||
vocabularyIncremental=false
|
||||
vocabularyUpdateMinWords=2000
|
||||
|
||||
[Homography]
|
||||
allCornersVisible=false
|
||||
confidence=0.995
|
||||
homographyComputed=true
|
||||
ignoreWhenAllInliers=false
|
||||
maxIterations=2000
|
||||
method="1:LMEDS;RANSAC;RHO"
|
||||
minAngle=50
|
||||
minimumInliers=10
|
||||
opticalFlow=false
|
||||
opticalFlowEps=0.01
|
||||
opticalFlowIterations=30
|
||||
opticalFlowMaxLevel=3
|
||||
opticalFlowWinSize=16
|
||||
ransacReprojThr=5
|
||||
rectBorderWidth=4
|
||||
|
||||
[NearestNeighbor]
|
||||
1Strategy="1:Linear;KDTree;KMeans;Composite;Autotuned;Lsh;BruteForce"
|
||||
2Distance_type="0:EUCLIDEAN_L2;MANHATTAN_L1;MINKOWSKI;MAX;HIST_INTERSECT;HELLINGER;CHI_SQUARE_CS;KULLBACK_LEIBLER_KL;HAMMING"
|
||||
3nndrRatioUsed=true
|
||||
4nndrRatio=0.8
|
||||
5minDistanceUsed=false
|
||||
6minDistance=1.6
|
||||
7ConvertBinToFloat=false
|
||||
7search_checks=32
|
||||
8search_eps=0
|
||||
9search_sorted=true
|
||||
Autotuned_build_weight=0.01
|
||||
Autotuned_memory_weight=0
|
||||
Autotuned_sample_fraction=0.1
|
||||
Autotuned_target_precision=0.8
|
||||
BruteForce_gpu=false
|
||||
Composite_branching=32
|
||||
Composite_cb_index=0.2
|
||||
Composite_centers_init="0:RANDOM;GONZALES;KMEANSPP"
|
||||
Composite_iterations=11
|
||||
Composite_trees=4
|
||||
KDTree_trees=4
|
||||
KMeans_branching=32
|
||||
KMeans_cb_index=0.2
|
||||
KMeans_centers_init="0:RANDOM;GONZALES;KMEANSPP"
|
||||
KMeans_iterations=11
|
||||
Lsh_key_size=20
|
||||
Lsh_multi_probe_level=2
|
||||
Lsh_table_number=12
|
||||
search_checks=32
|
||||
search_eps=0
|
||||
search_sorted=true
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 23 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 19 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 18 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 13 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 16 KiB |
@@ -0,0 +1,87 @@
|
||||
|
||||
# Requires installed https://github.com/chvmp/champ/tree/ros2
|
||||
#
|
||||
# Example:
|
||||
# 1) Launch simulator (gazebo, nav2 and rtabmap):
|
||||
# $ ros2 launch rtabmap_demos champ_sim_vslam.launch.py
|
||||
#
|
||||
# Note that the first time we launch gazebo, it may take a
|
||||
# while to download all assets. You may need to restart the
|
||||
# launch to make sure all nodes are started after the sim is ready.
|
||||
#
|
||||
# 2) Move the robot:
|
||||
# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
|
||||
# a) By teleoperating:
|
||||
# $ ros2 launch champ_teleop teleop.launch.py
|
||||
#
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
import os
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
|
||||
sim_launch_path = PathJoinSubstitution(
|
||||
[FindPackageShare('champ_config'), 'launch', 'gazebo.launch.py']
|
||||
)
|
||||
|
||||
gz_pkg_share = FindPackageShare(package="champ_gazebo").find("champ_gazebo")
|
||||
|
||||
champ_vslam = PathJoinSubstitution(
|
||||
[FindPackageShare('rtabmap_demos'), 'launch', 'champ', 'champ_vslam.launch.py']
|
||||
)
|
||||
|
||||
rviz = LaunchConfiguration('rviz').perform(context)
|
||||
world = LaunchConfiguration('world').perform(context)
|
||||
|
||||
return [
|
||||
TimerAction(
|
||||
actions = [
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(champ_vslam),
|
||||
launch_arguments={
|
||||
'use_sim_time': 'true',
|
||||
'rviz': rviz,
|
||||
'rtabmap_viz': LaunchConfiguration('rtabmap_viz'),
|
||||
'localization': LaunchConfiguration('localization'),
|
||||
}.items()
|
||||
)], period = 5.0), # Wait 5 sec to make sure simulator is ready
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(sim_launch_path),
|
||||
launch_arguments={'rviz': 'false',
|
||||
'world': os.path.join(gz_pkg_share, f"worlds/{world}.world")}.items()
|
||||
),
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
DeclareLaunchArgument(
|
||||
name='rviz',
|
||||
default_value='true',
|
||||
description='Run rviz'
|
||||
),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
name='rtabmap_viz',
|
||||
default_value='true',
|
||||
description='Run rtabmap_viz'
|
||||
),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false', choices=['true', 'false'],
|
||||
description='Launch rtabmap in localization mode (a map should have been already created).'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'world', default_value='playground',
|
||||
choices=['outdoor', 'playground'],
|
||||
description='Champ gazebo world.'),
|
||||
|
||||
OpaqueFunction(function=launch_setup)
|
||||
])
|
||||
@@ -0,0 +1,179 @@
|
||||
|
||||
# Similar to gazebo example on https://github.com/chvmp/champ/tree/ros2, we can do:
|
||||
#
|
||||
# Run the Gazebo environment:
|
||||
# $ ros2 launch champ_config gazebo.launch.py
|
||||
#
|
||||
# Run Nav2's navigation and rtabmap:
|
||||
# $ ros2 launch rtabmap_demos champ_vslam.launch.py use_sim_time:=true rviz:=true rtabmap_viz:=true
|
||||
#
|
||||
# When a map is already created using command above, we can re-launch in localization-only mode with:
|
||||
# $ ros2 launch rtabmap_demos champ_vslam.launch.py use_sim_time:=true rviz:=true rtabmap_viz:=true localization:=true
|
||||
#
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
|
||||
localization = LaunchConfiguration('localization')
|
||||
|
||||
navigation_launch_path = PathJoinSubstitution(
|
||||
[FindPackageShare('nav2_bringup'), 'launch', 'navigation_launch.py']
|
||||
)
|
||||
|
||||
nav2_params_file = PathJoinSubstitution(
|
||||
[FindPackageShare('rtabmap_demos'), 'params', 'champ_nav2_params.yaml']
|
||||
)
|
||||
|
||||
rviz_config_path = PathJoinSubstitution(
|
||||
[FindPackageShare('champ_navigation'), 'rviz', 'navigation.rviz']
|
||||
)
|
||||
|
||||
use_sim_time = LaunchConfiguration("use_sim_time")
|
||||
|
||||
# With the simulator, the imu is not published fast enough
|
||||
# and have a huge delay, disabling imu usage from VO
|
||||
use_imu = use_sim_time.perform(context) in ["false", "False"]
|
||||
|
||||
vslam_params ={
|
||||
'frame_id':'base_link',
|
||||
'guess_frame_id':'odom',
|
||||
'approx_sync': False,
|
||||
'use_sim_time':use_sim_time,
|
||||
'subscribe_rgbd':True,
|
||||
'subscribe_odom_info':True,
|
||||
'use_action_for_goal':True,
|
||||
'wait_imu_to_init': use_imu,
|
||||
'wait_for_transform': 0.5,
|
||||
# RTAB-Map's parameters should be strings
|
||||
'Grid/DepthDecimation': '1',
|
||||
'Grid/RangeMax': '2',
|
||||
'GridGlobal/MinSize': '20',
|
||||
'Grid/MinClusterSize': '20',
|
||||
'Grid/MaxObstacleHeight': '2',
|
||||
'Odom/ResetCountdown': '2', # sim is very flaky
|
||||
'Kp/RoiRatios': '0.0 0.0 0.0 0.4' # ignore ground for loop closure detection (sim uses a very repetitive texture)
|
||||
}
|
||||
|
||||
vslam_remappings=[('imu', 'imu/data/filtered'),
|
||||
('odom', 'vo')]
|
||||
|
||||
return [
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(navigation_launch_path),
|
||||
launch_arguments={
|
||||
'use_sim_time': use_sim_time,
|
||||
'params_file': nav2_params_file
|
||||
}.items()
|
||||
),
|
||||
|
||||
Node(
|
||||
package='rviz2',
|
||||
executable='rviz2',
|
||||
name='rviz2',
|
||||
output='screen',
|
||||
arguments=['-d', rviz_config_path],
|
||||
condition=IfCondition(LaunchConfiguration("rviz")),
|
||||
parameters=[{'use_sim_time': use_sim_time}]
|
||||
),
|
||||
|
||||
# compute imu orientation
|
||||
Node(
|
||||
package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
|
||||
parameters=[{
|
||||
'use_mag':False,
|
||||
'world_frame':'enu',
|
||||
'publish_tf':False}],
|
||||
remappings=[
|
||||
('imu/data_raw', 'imu/data'),
|
||||
('imu/data', 'imu/data/filtered')
|
||||
]),
|
||||
|
||||
# VSLAM nodes:
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
parameters=[vslam_params],
|
||||
remappings=[('rgb/image', '/camera/image_raw'),
|
||||
('rgb/camera_info', '/camera/camera_info'),
|
||||
('depth/image', '/camera/depth/image_raw')]),
|
||||
|
||||
Node(
|
||||
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
|
||||
parameters=[vslam_params, {'odom_frame_id': 'vo'}],
|
||||
remappings=vslam_remappings,
|
||||
arguments=["--ros-args", "--log-level", 'info']),
|
||||
|
||||
# SLAM Mode:
|
||||
Node(
|
||||
condition=UnlessCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[vslam_params],
|
||||
remappings=vslam_remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
# Localization mode:
|
||||
Node(
|
||||
condition=IfCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[vslam_params,
|
||||
{'Mem/IncrementalMemory':'False',
|
||||
'Mem/InitWMWithAllNodes':'True'}],
|
||||
remappings=vslam_remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
condition=IfCondition(LaunchConfiguration("rtabmap_viz")),
|
||||
parameters=[vslam_params],
|
||||
remappings=vslam_remappings),
|
||||
|
||||
# Compute ground/obstacle clouds for nav2 voxel layers
|
||||
Node(
|
||||
package='rtabmap_util', executable='point_cloud_xyz', output='screen',
|
||||
parameters=[{'decimation': 2,
|
||||
'max_depth': 3.0,
|
||||
'voxel_size': 0.02}],
|
||||
remappings=[('depth/image', '/camera/depth/image_raw'),
|
||||
('depth/camera_info', '/camera/depth/camera_info'),
|
||||
('cloud', '/camera/cloud')]),
|
||||
|
||||
Node(
|
||||
package='rtabmap_util', executable='obstacles_detection', output='screen',
|
||||
parameters=[vslam_params],
|
||||
remappings=[('cloud', '/camera/cloud'),
|
||||
('obstacles', '/camera/obstacles'),
|
||||
('ground', '/camera/ground')]),
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
name='use_sim_time',
|
||||
default_value='false',
|
||||
description='Enable use_sime_time to true'
|
||||
),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
name='rviz',
|
||||
default_value='false',
|
||||
description='Run rviz'
|
||||
),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
name='rtabmap_viz',
|
||||
default_value='false',
|
||||
description='Run rtabmap_viz'
|
||||
),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false', choices=['true', 'false'],
|
||||
description='Launch rtabmap in localization mode (a map should have been already created).'),
|
||||
|
||||
OpaqueFunction(function=launch_setup)
|
||||
])
|
||||
@@ -0,0 +1,129 @@
|
||||
# Requirements:
|
||||
# find_object_2d package installed
|
||||
# Download rosbag:
|
||||
# * demo_find_object.db3: https://drive.google.com/file/d/1web54yQkxeGFr2UwOjKeoajGGDm0fZXT/view?usp=drive_link
|
||||
#
|
||||
# Example:
|
||||
#
|
||||
# SLAM:
|
||||
# $ ros2 launch rtabmap_demos find_object_demo.launch.py
|
||||
#
|
||||
# Rosbag:
|
||||
# $ ros2 bag play demo_find_object.db3 --clock
|
||||
#
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.actions import SetParameter
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
localization = LaunchConfiguration('localization')
|
||||
|
||||
parameters={
|
||||
'frame_id':'base_footprint',
|
||||
'odom_frame_id':'odom',
|
||||
'odom_tf_linear_variance':0.001,
|
||||
'odom_tf_angular_variance':0.001,
|
||||
'subscribe_rgbd':True,
|
||||
'subscribe_scan':True,
|
||||
'approx_sync':True,
|
||||
'sync_queue_size': 10,
|
||||
# RTAB-Map's internal parameters should be strings
|
||||
'RGBD/NeighborLinkRefining': 'true', # Do odometry correction with consecutive laser scans
|
||||
'Reg/Strategy': '1', # 0=Visual, 1=ICP, 2=Visual+ICP
|
||||
'Reg/Force3DoF': 'true', # 2D SLAM
|
||||
}
|
||||
|
||||
remappings=[
|
||||
('rgb/image', '/camera/data_throttled_image'),
|
||||
('depth/image', '/camera/data_throttled_image_depth'),
|
||||
('rgb/camera_info', '/camera/data_throttled_camera_info'),
|
||||
('scan', '/base_scan')]
|
||||
|
||||
config_rviz = os.path.join(
|
||||
get_package_share_directory('rtabmap_demos'), 'config', 'demo_robot_mapping.rviz'
|
||||
)
|
||||
|
||||
config_find_object = os.path.join(
|
||||
get_package_share_directory('rtabmap_demos'), 'config', 'find_object.ini'
|
||||
)
|
||||
|
||||
data_find_object = os.path.join(
|
||||
get_package_share_directory('rtabmap_demos'), 'data', 'books'
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument('rtabmap_viz', default_value='false', description='Launch RTAB-Map UI (optional).'),
|
||||
DeclareLaunchArgument('rviz', default_value='true', description='Launch RVIZ (optional).'),
|
||||
DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'),
|
||||
DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'),
|
||||
|
||||
SetParameter(name='use_sim_time', value=True),
|
||||
|
||||
# Nodes to launch
|
||||
|
||||
# Uncompress images for find_object
|
||||
Node(
|
||||
package='image_transport', executable='republish', name='republish_rgb', output='screen',
|
||||
arguments=['compressed', 'raw'],
|
||||
remappings=[('in/compressed', '/camera/data_throttled_image/compressed'),
|
||||
('out', '/camera/data_throttled_image')]),
|
||||
Node(
|
||||
package='image_transport', executable='republish', name='republish_depth', output='screen',
|
||||
arguments=['compressedDepth', 'raw'],
|
||||
remappings=[('in/compressedDepth', '/camera/data_throttled_image_depth/compressedDepth'),
|
||||
('out', '/camera/data_throttled_image_depth')]),
|
||||
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
parameters=[parameters,
|
||||
{'approx_sync_max_interval': 0.02}],
|
||||
remappings=remappings),
|
||||
|
||||
# SLAM mode:
|
||||
Node(
|
||||
condition=UnlessCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=remappings,
|
||||
arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db)
|
||||
|
||||
# Localization mode:
|
||||
Node(
|
||||
condition=IfCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[parameters,
|
||||
{'Mem/IncrementalMemory':'False',
|
||||
'Mem/InitWMWithAllNodes':'True'}],
|
||||
remappings=remappings),
|
||||
|
||||
# Visualization:
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
condition=IfCondition(LaunchConfiguration("rtabmap_viz")),
|
||||
parameters=[parameters],
|
||||
remappings=remappings),
|
||||
Node(
|
||||
package='rviz2', executable='rviz2', name="rviz2", output='screen',
|
||||
condition=IfCondition(LaunchConfiguration("rviz")),
|
||||
arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]),
|
||||
|
||||
# Find-Object
|
||||
Node(
|
||||
package='find_object_2d', executable='find_object_2d', output='screen',
|
||||
parameters=[{'gui': True,
|
||||
'subscribe_depth': True,
|
||||
'settings_path': config_find_object,
|
||||
'objects_path': data_find_object}],
|
||||
remappings=[('rgb/image_rect_color', '/camera/data_throttled_image'),
|
||||
('depth_registered/image_raw', '/camera/data_throttled_image_depth'),
|
||||
('depth_registered/camera_info', '/camera/data_throttled_camera_info')]),
|
||||
])
|
||||
@@ -0,0 +1,104 @@
|
||||
#
|
||||
# Requirements:
|
||||
# - Install: ros-$ROS_DISTRO-clearpath-simulator ros-$ROS_DISTRO-clearpath-nav2-demos ros-$ROS_DISTRO-clearpath-config ros-$ROS_DISTRO-moveit-setup-srdf-plugins
|
||||
# - Copy /opt/ros/humble/share/clearpath_config/sample/a200_sample.yaml to ~/clearpath/robot.yaml
|
||||
# - Fix camera intrinsics by editing /opt/ros/humble/share/clearpath_sensors_description/urdf/intel_realsense.urdf.xacro:
|
||||
# <horizontal_fov>1.047</horizontal_fov>
|
||||
# <image>
|
||||
# <width>320</width>
|
||||
# <height>240</height>
|
||||
# </image>
|
||||
#
|
||||
# Example with gazebo:
|
||||
# 1) Launch simulator (husky, nav2 and rtabmap):
|
||||
# $ ros2 launch rtabmap_demos husky_sim_scan2d_demo.launch.py robot_ns:=a200_0000
|
||||
#
|
||||
# 2) Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec.
|
||||
#
|
||||
# 3) Move the robot:
|
||||
# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
|
||||
# a) By teleoperating:
|
||||
# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel
|
||||
#
|
||||
|
||||
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, PathJoinSubstitution
|
||||
|
||||
import os
|
||||
|
||||
ARGUMENTS = [
|
||||
DeclareLaunchArgument('rtabmap_viz', default_value='true',
|
||||
choices=['true', 'false'], description='Start rtabmap_viz.'),
|
||||
DeclareLaunchArgument('localization', default_value='false',
|
||||
choices=['true', 'false'], description='Start rtabmap in localization mode (a map should have been already created).'),
|
||||
DeclareLaunchArgument('world', default_value='warehouse',
|
||||
description='Ignition World'),
|
||||
DeclareLaunchArgument('robot_ns', default_value='a200_0000',
|
||||
description='Robot namespace'),
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
# Directories
|
||||
pkg_clearpath_gz = get_package_share_directory(
|
||||
'clearpath_gz')
|
||||
pkg_clearpath_viz = get_package_share_directory(
|
||||
'clearpath_viz')
|
||||
pkg_rtabmap_demos = get_package_share_directory(
|
||||
'rtabmap_demos')
|
||||
pkg_clearpath_nav2_demos = get_package_share_directory(
|
||||
'clearpath_nav2_demos')
|
||||
|
||||
# Paths
|
||||
sim_launch = PathJoinSubstitution(
|
||||
[pkg_clearpath_gz, 'launch', 'simulation.launch.py'])
|
||||
viz_launch = PathJoinSubstitution(
|
||||
[pkg_clearpath_viz, 'launch', 'view_navigation.launch.py'])
|
||||
rtabmap_launch = PathJoinSubstitution(
|
||||
[pkg_rtabmap_demos, 'launch', 'husky', 'husky_slam2d.launch.py'])
|
||||
nav2_launch = PathJoinSubstitution(
|
||||
[pkg_clearpath_nav2_demos, 'launch', 'nav2.launch.py'])
|
||||
|
||||
sim = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([sim_launch]),
|
||||
launch_arguments=[
|
||||
('world', LaunchConfiguration('world')),
|
||||
]
|
||||
)
|
||||
|
||||
viz = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([viz_launch]),
|
||||
launch_arguments=[
|
||||
('namespace', LaunchConfiguration('robot_ns')),
|
||||
]
|
||||
)
|
||||
|
||||
rtabmap = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([rtabmap_launch]),
|
||||
launch_arguments=[
|
||||
('rtabmap_viz', LaunchConfiguration('rtabmap_viz')),
|
||||
('localization', LaunchConfiguration('localization')),
|
||||
('use_sim_time', 'true'),
|
||||
('robot_ns', LaunchConfiguration('robot_ns'))
|
||||
]
|
||||
)
|
||||
|
||||
nav2 = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([nav2_launch]),
|
||||
launch_arguments=[
|
||||
('setup_path', os.path.expanduser('~')+'/clearpath/'),
|
||||
('use_sim_time', 'true'),
|
||||
]
|
||||
)
|
||||
|
||||
# Create launch description and add actions
|
||||
ld = LaunchDescription(ARGUMENTS)
|
||||
ld.add_action(rtabmap)
|
||||
ld.add_action(sim)
|
||||
ld.add_action(viz)
|
||||
ld.add_action(nav2)
|
||||
return ld
|
||||
@@ -0,0 +1,104 @@
|
||||
#
|
||||
# Requirements:
|
||||
# - Install: ros-$ROS_DISTRO-clearpath-simulator ros-$ROS_DISTRO-clearpath-nav2-demos ros-$ROS_DISTRO-clearpath-config ros-$ROS_DISTRO-moveit-setup-srdf-plugins
|
||||
# - Copy /opt/ros/humble/share/clearpath_config/sample/a200_sample.yaml to ~/clearpath/robot.yaml
|
||||
# - Fix camera intrinsics by editing /opt/ros/humble/share/clearpath_sensors_description/urdf/intel_realsense.urdf.xacro:
|
||||
# <horizontal_fov>1.047</horizontal_fov>
|
||||
# <image>
|
||||
# <width>320</width>
|
||||
# <height>240</height>
|
||||
# </image>
|
||||
# - Fix lidar sim distortions by editing /opt/ros/humble/share/clearpath_gz/worlds/warehouse.sdf (https://github.com/gazebosim/gz-sim/issues/2743):
|
||||
# - <render_engine>ogre2</render_engine>
|
||||
# + <render_engine>ogre</render_engine>
|
||||
#
|
||||
# Example with gazebo:
|
||||
# 1) Launch simulator (husky, nav2 and rtabmap):
|
||||
# $ ros2 launch rtabmap_demos husky_sim_scan3d_assemble_demo.launch.py robot_ns:=a200_0000
|
||||
#
|
||||
# 2) Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec.
|
||||
#
|
||||
# 3) Move the robot:
|
||||
# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
|
||||
# a) By teleoperating:
|
||||
# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel
|
||||
#
|
||||
|
||||
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, PathJoinSubstitution
|
||||
|
||||
import os
|
||||
|
||||
ARGUMENTS = [
|
||||
DeclareLaunchArgument('rtabmap_viz', default_value='true',
|
||||
choices=['true', 'false'], description='Start rtabmap_viz.'),
|
||||
DeclareLaunchArgument('world', default_value='warehouse',
|
||||
description='Ignition World'),
|
||||
DeclareLaunchArgument('robot_ns', default_value='a200_0000',
|
||||
description='Robot namespace'),
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
# Directories
|
||||
pkg_clearpath_gz = get_package_share_directory(
|
||||
'clearpath_gz')
|
||||
pkg_clearpath_viz = get_package_share_directory(
|
||||
'clearpath_viz')
|
||||
pkg_rtabmap_demos = get_package_share_directory(
|
||||
'rtabmap_demos')
|
||||
pkg_clearpath_nav2_demos = get_package_share_directory(
|
||||
'clearpath_nav2_demos')
|
||||
|
||||
# Paths
|
||||
sim_launch = PathJoinSubstitution(
|
||||
[pkg_clearpath_gz, 'launch', 'simulation.launch.py'])
|
||||
viz_launch = PathJoinSubstitution(
|
||||
[pkg_clearpath_viz, 'launch', 'view_navigation.launch.py'])
|
||||
rtabmap_launch = PathJoinSubstitution(
|
||||
[pkg_rtabmap_demos, 'launch', 'husky', 'husky_slam3d_assemble.launch.py'])
|
||||
nav2_launch = PathJoinSubstitution(
|
||||
[pkg_clearpath_nav2_demos, 'launch', 'nav2.launch.py'])
|
||||
|
||||
sim = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([sim_launch]),
|
||||
launch_arguments=[
|
||||
('world', LaunchConfiguration('world')),
|
||||
]
|
||||
)
|
||||
|
||||
viz = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([viz_launch]),
|
||||
launch_arguments=[
|
||||
('namespace', LaunchConfiguration('robot_ns')),
|
||||
]
|
||||
)
|
||||
|
||||
rtabmap = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([rtabmap_launch]),
|
||||
launch_arguments=[
|
||||
('rtabmap_viz', LaunchConfiguration('rtabmap_viz')),
|
||||
('use_sim_time', 'true'),
|
||||
('robot_ns', LaunchConfiguration('robot_ns'))
|
||||
]
|
||||
)
|
||||
|
||||
nav2 = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([nav2_launch]),
|
||||
launch_arguments=[
|
||||
('setup_path', os.path.expanduser('~')+'/clearpath/'),
|
||||
('use_sim_time', 'true'),
|
||||
]
|
||||
)
|
||||
|
||||
# Create launch description and add actions
|
||||
ld = LaunchDescription(ARGUMENTS)
|
||||
ld.add_action(rtabmap)
|
||||
ld.add_action(sim)
|
||||
ld.add_action(viz)
|
||||
ld.add_action(nav2)
|
||||
return ld
|
||||
@@ -0,0 +1,110 @@
|
||||
#
|
||||
# Requirements:
|
||||
# - Install: ros-$ROS_DISTRO-clearpath-simulator ros-$ROS_DISTRO-clearpath-nav2-demos ros-$ROS_DISTRO-clearpath-config ros-$ROS_DISTRO-moveit-setup-srdf-plugins
|
||||
# - Copy /opt/ros/humble/share/clearpath_config/sample/a200_sample.yaml to ~/clearpath/robot.yaml
|
||||
# - Fix camera intrinsics by editing /opt/ros/humble/share/clearpath_sensors_description/urdf/intel_realsense.urdf.xacro:
|
||||
# <horizontal_fov>1.047</horizontal_fov>
|
||||
# <image>
|
||||
# <width>320</width>
|
||||
# <height>240</height>
|
||||
# </image>
|
||||
# - Fix lidar sim distortions by editing /opt/ros/humble/share/clearpath_gz/worlds/warehouse.sdf (https://github.com/gazebosim/gz-sim/issues/2743):
|
||||
# - <render_engine>ogre2</render_engine>
|
||||
# + <render_engine>ogre</render_engine>
|
||||
#
|
||||
# Example with gazebo:
|
||||
# 1) Launch simulator (husky, nav2 and rtabmap):
|
||||
# $ ros2 launch rtabmap_demos husky_sim_scan3d_demo.launch.py robot_ns:=a200_0000
|
||||
#
|
||||
# 2) Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec.
|
||||
#
|
||||
# 3) Move the robot:
|
||||
# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
|
||||
# a) By teleoperating:
|
||||
# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel
|
||||
#
|
||||
|
||||
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, PathJoinSubstitution
|
||||
|
||||
import os
|
||||
|
||||
ARGUMENTS = [
|
||||
DeclareLaunchArgument('rtabmap_viz', default_value='true',
|
||||
choices=['true', 'false'], description='Start rtabmap_viz.'),
|
||||
DeclareLaunchArgument('localization', default_value='false',
|
||||
choices=['true', 'false'], description='Start rtabmap in localization mode (a map should have been already created).'),
|
||||
DeclareLaunchArgument('world', default_value='warehouse',
|
||||
description='Ignition World'),
|
||||
DeclareLaunchArgument('robot_ns', default_value='a200_0000',
|
||||
description='Robot namespace'),
|
||||
DeclareLaunchArgument('use_camera', default_value='true',
|
||||
description='Use camera for global loop closure / re-localization.'),
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
# Directories
|
||||
pkg_clearpath_gz = get_package_share_directory(
|
||||
'clearpath_gz')
|
||||
pkg_clearpath_viz = get_package_share_directory(
|
||||
'clearpath_viz')
|
||||
pkg_rtabmap_demos = get_package_share_directory(
|
||||
'rtabmap_demos')
|
||||
pkg_clearpath_nav2_demos = get_package_share_directory(
|
||||
'clearpath_nav2_demos')
|
||||
|
||||
# Paths
|
||||
sim_launch = PathJoinSubstitution(
|
||||
[pkg_clearpath_gz, 'launch', 'simulation.launch.py'])
|
||||
viz_launch = PathJoinSubstitution(
|
||||
[pkg_clearpath_viz, 'launch', 'view_navigation.launch.py'])
|
||||
rtabmap_launch = PathJoinSubstitution(
|
||||
[pkg_rtabmap_demos, 'launch', 'husky', 'husky_slam3d.launch.py'])
|
||||
nav2_launch = PathJoinSubstitution(
|
||||
[pkg_clearpath_nav2_demos, 'launch', 'nav2.launch.py'])
|
||||
|
||||
sim = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([sim_launch]),
|
||||
launch_arguments=[
|
||||
('world', LaunchConfiguration('world')),
|
||||
]
|
||||
)
|
||||
|
||||
viz = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([viz_launch]),
|
||||
launch_arguments=[
|
||||
('namespace', LaunchConfiguration('robot_ns')),
|
||||
]
|
||||
)
|
||||
|
||||
rtabmap = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([rtabmap_launch]),
|
||||
launch_arguments=[
|
||||
('rtabmap_viz', LaunchConfiguration('rtabmap_viz')),
|
||||
('localization', LaunchConfiguration('localization')),
|
||||
('use_sim_time', 'true'),
|
||||
('use_camera', LaunchConfiguration('use_camera')),
|
||||
('robot_ns', LaunchConfiguration('robot_ns'))
|
||||
]
|
||||
)
|
||||
|
||||
nav2 = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([nav2_launch]),
|
||||
launch_arguments=[
|
||||
('setup_path', os.path.expanduser('~')+'/clearpath/'),
|
||||
('use_sim_time', 'true'),
|
||||
]
|
||||
)
|
||||
|
||||
# Create launch description and add actions
|
||||
ld = LaunchDescription(ARGUMENTS)
|
||||
ld.add_action(rtabmap)
|
||||
ld.add_action(sim)
|
||||
ld.add_action(viz)
|
||||
ld.add_action(nav2)
|
||||
return ld
|
||||
@@ -0,0 +1,128 @@
|
||||
#
|
||||
#
|
||||
# Example with gazebo:
|
||||
# 1) Launch simulator (husky):
|
||||
# $ ros2 launch clearpath_gz simulation.launch.py
|
||||
# Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec.
|
||||
#
|
||||
# 2) Launch rviz:
|
||||
# $ ros2 launch clearpath_viz view_navigation.launch.py namespace:=a200_0000
|
||||
#
|
||||
# 3) Launch SLAM:
|
||||
# $ ros2 launch rtabmap_demos husky_slam2d.launch.py use_sim_time:=true
|
||||
#
|
||||
# 4) Launch nav2"
|
||||
# $ ros2 launch clearpath_nav2_demos nav2.launch.py setup_path:=$HOME/clearpath/ use_sim_time:=true
|
||||
#
|
||||
# 4) Click on "Play" button on bottom-left of gazebo.
|
||||
#
|
||||
# 5) Move the robot:
|
||||
# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
|
||||
# a) By teleoperating:
|
||||
# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel
|
||||
#
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
localization = LaunchConfiguration('localization')
|
||||
robot_ns = LaunchConfiguration('robot_ns')
|
||||
|
||||
icp_odom_parameters={
|
||||
'odom_frame_id':'icp_odom',
|
||||
'guess_frame_id':'odom'
|
||||
}
|
||||
|
||||
rtabmap_parameters={
|
||||
'subscribe_rgbd':True,
|
||||
'subscribe_scan':True,
|
||||
'use_action_for_goal':True,
|
||||
'odom_sensor_sync': True,
|
||||
# RTAB-Map's parameters should be strings:
|
||||
'Mem/NotLinkedNodesKept':'false',
|
||||
'Grid/RangeMin':'0.7', # ignore laser scan points on the robot itself
|
||||
'RGBD/OptimizeMaxError':'2',
|
||||
}
|
||||
|
||||
# Shared parameters between different nodes
|
||||
shared_parameters={
|
||||
'frame_id':'base_link',
|
||||
'use_sim_time':use_sim_time,
|
||||
# RTAB-Map's parameters should be strings:
|
||||
'Reg/Strategy':'1',
|
||||
'Reg/Force3DoF':'true', # we are moving on a 2D flat floor
|
||||
'Mem/NotLinkedNodesKept':'false',
|
||||
'Icp/PointToPlaneMinComplexity':'0.04', # to be more robust to long corridors with low geometry
|
||||
'Icp/MaxTranslation': '1'
|
||||
}
|
||||
|
||||
remappings=[
|
||||
('/tf', 'tf'),
|
||||
('/tf_static', 'tf_static'),
|
||||
('odom', 'icp_odom'),
|
||||
('scan', 'sensors/lidar2d_0/scan'),
|
||||
('rgb/image', 'sensors/camera_0/color/image'),
|
||||
('rgb/camera_info', 'sensors/camera_0/color/camera_info'),
|
||||
('depth/image', 'sensors/camera_0/depth/image')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='false', choices=['true', 'false'],
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false', choices=['true', 'false'],
|
||||
description='Launch rtabmap in localization mode (a map should have been already created).'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'robot_ns', default_value='a200_0000',
|
||||
description='Robot namespace.'),
|
||||
|
||||
# Nodes to launch
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}],
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_odom', executable='icp_odometry', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[icp_odom_parameters, shared_parameters],
|
||||
remappings=remappings,
|
||||
arguments=["--ros-args", "--log-level", 'warn']),
|
||||
|
||||
# SLAM Mode:
|
||||
Node(
|
||||
condition=UnlessCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[rtabmap_parameters, shared_parameters],
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
# Localization mode:
|
||||
Node(
|
||||
condition=IfCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[rtabmap_parameters, shared_parameters,
|
||||
{'Mem/IncrementalMemory':'False',
|
||||
'Mem/InitWMWithAllNodes':'True'}],
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[rtabmap_parameters, shared_parameters],
|
||||
remappings=remappings),
|
||||
])
|
||||
@@ -0,0 +1,146 @@
|
||||
#
|
||||
#
|
||||
# Example with gazebo:
|
||||
# 1) Launch simulator (husky):
|
||||
# $ ros2 launch clearpath_gz simulation.launch.py
|
||||
# Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec.
|
||||
#
|
||||
# 2) Launch rviz:
|
||||
# $ ros2 launch clearpath_viz view_navigation.launch.py namespace:=a200_0000
|
||||
#
|
||||
# 3) Launch SLAM:
|
||||
# $ ros2 launch rtabmap_demos husky_slam3d.launch.py use_sim_time:=true
|
||||
#
|
||||
# 4) Launch nav2"
|
||||
# $ ros2 launch clearpath_nav2_demos nav2.launch.py setup_path:=$HOME/clearpath/ use_sim_time:=true
|
||||
#
|
||||
# 4) Click on "Play" button on bottom-left of gazebo.
|
||||
#
|
||||
# 5) Move the robot:
|
||||
# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
|
||||
# a) By teleoperating:
|
||||
# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel
|
||||
#
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
localization = LaunchConfiguration('localization')
|
||||
robot_ns = LaunchConfiguration('robot_ns')
|
||||
use_camera = LaunchConfiguration('use_camera')
|
||||
|
||||
icp_odom_parameters={
|
||||
'odom_frame_id':'icp_odom',
|
||||
'guess_frame_id':'odom',
|
||||
'OdomF2M/ScanSubtractRadius': '0.3', # match voxel size
|
||||
'OdomF2M/ScanMaxSize': '10000'
|
||||
}
|
||||
|
||||
rtabmap_parameters={
|
||||
'subscribe_rgb':False,
|
||||
'subscribe_depth':False,
|
||||
'subscribe_rgbd': use_camera,
|
||||
'subscribe_scan_cloud':True,
|
||||
'use_action_for_goal':True,
|
||||
'odom_sensor_sync': True,
|
||||
# RTAB-Map's parameters should be strings:
|
||||
'Mem/NotLinkedNodesKept':'false',
|
||||
'Grid/RangeMin':'0.5', # ignore laser scan points on the robot itself
|
||||
'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles
|
||||
'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles
|
||||
'Grid/MaxObstacleHeight':'1', # All points over 1 meter are ignored
|
||||
'Grid/RayTracing':'true', # Fill empty space
|
||||
'Grid/3D':'false', # Use 2D occupancy
|
||||
'RGBD/OptimizeMaxError':'0.3', # There are a lot of repetitive patterns, be more strict in accepting loop closures
|
||||
}
|
||||
|
||||
# Shared parameters between different nodes
|
||||
shared_parameters={
|
||||
'frame_id':'base_link',
|
||||
'use_sim_time':use_sim_time,
|
||||
# RTAB-Map's parameters should be strings:
|
||||
'Reg/Strategy':'1',
|
||||
'Reg/Force3DoF':'true', # we are moving on a 2D flat floor
|
||||
'Mem/NotLinkedNodesKept':'false',
|
||||
'Icp/VoxelSize': '0.3',
|
||||
'Icp/MaxCorrespondenceDistance': '3', # roughly 10x voxel size
|
||||
'Icp/PointToPlaneGroundNormalsUp': '0.9',
|
||||
'Icp/RangeMin': '0.5',
|
||||
'Icp/MaxTranslation': '1'
|
||||
}
|
||||
|
||||
remappings=[
|
||||
('/tf', 'tf'),
|
||||
('/tf_static', 'tf_static'),
|
||||
('odom', 'icp_odom'),
|
||||
('scan_cloud', 'sensors/lidar3d_0/points'),
|
||||
('rgb/image', 'sensors/camera_0/color/image'),
|
||||
('rgb/camera_info', 'sensors/camera_0/color/camera_info'),
|
||||
('depth/image', 'sensors/camera_0/depth/image')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='false', choices=['true', 'false'],
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false', choices=['true', 'false'],
|
||||
description='Launch rtabmap in localization mode (a map should have been already created).'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'robot_ns', default_value='a200_0000',
|
||||
description='Robot namespace.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'use_camera', default_value='true',
|
||||
description='Use camera for global loop closure / re-localization.'),
|
||||
|
||||
# Nodes to launch
|
||||
Node(
|
||||
condition=IfCondition(use_camera),
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}],
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_odom', executable='icp_odometry', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[icp_odom_parameters, shared_parameters],
|
||||
remappings=remappings,
|
||||
arguments=["--ros-args", "--log-level", 'warn']),
|
||||
|
||||
# SLAM Mode:
|
||||
Node(
|
||||
condition=UnlessCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[rtabmap_parameters, shared_parameters],
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
# Localization mode:
|
||||
Node(
|
||||
condition=IfCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[rtabmap_parameters, shared_parameters,
|
||||
{'Mem/IncrementalMemory':'False',
|
||||
'Mem/InitWMWithAllNodes':'True'}],
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[rtabmap_parameters, shared_parameters],
|
||||
remappings=remappings),
|
||||
])
|
||||
@@ -0,0 +1,134 @@
|
||||
#
|
||||
#
|
||||
# Example with gazebo:
|
||||
# 1) Launch simulator (husky):
|
||||
# $ ros2 launch clearpath_gz simulation.launch.py
|
||||
# Click on "Play" button on bottom-left of gazebo as soon as you can see it to avoid controllers crashing after 5 sec.
|
||||
#
|
||||
# 2) Launch rviz:
|
||||
# $ ros2 launch clearpath_viz view_navigation.launch.py namespace:=a200_0000
|
||||
#
|
||||
# 3) Launch SLAM:
|
||||
# $ ros2 launch rtabmap_demos husky_slam3d_assemble.launch.py use_sim_time:=true
|
||||
#
|
||||
# 4) Launch nav2"
|
||||
# $ ros2 launch clearpath_nav2_demos nav2.launch.py setup_path:=$HOME/clearpath/ use_sim_time:=true
|
||||
#
|
||||
# 4) Click on "Play" button on bottom-left of gazebo.
|
||||
#
|
||||
# 5) Move the robot:
|
||||
# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
|
||||
# a) By teleoperating:
|
||||
# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=/a200_0000/cmd_vel
|
||||
#
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
robot_ns = LaunchConfiguration('robot_ns')
|
||||
|
||||
icp_odom_parameters={
|
||||
'odom_frame_id':'icp_odom',
|
||||
'guess_frame_id':'odom',
|
||||
'OdomF2M/ScanSubtractRadius': '0.3', # match voxel size
|
||||
'OdomF2M/ScanMaxSize': '10000'
|
||||
}
|
||||
|
||||
rtabmap_parameters={
|
||||
'subscribe_rgbd':True,
|
||||
'subscribe_depth':False,
|
||||
'subscribe_rgb':False,
|
||||
'subscribe_scan_cloud':True,
|
||||
'use_action_for_goal':True,
|
||||
'odom_sensor_sync': True,
|
||||
'topic_queue_size': 30,
|
||||
'sync_queue_size': 30,
|
||||
'approx_sync': True,
|
||||
'qos': 1,
|
||||
# RTAB-Map's parameters should be strings:
|
||||
'Mem/NotLinkedNodesKept':'false',
|
||||
'Grid/RangeMin':'0.5', # ignore laser scan points on the robot itself
|
||||
'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles
|
||||
'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles
|
||||
'Grid/MaxObstacleHeight':'1', # All points over 1 meter are ignored
|
||||
'Grid/RayTracing':'true', # Fill empty space
|
||||
'Grid/3D':'false', # Use 2D occupancy
|
||||
'RGBD/OptimizeMaxError':'0.3', # There are a lot of repetitive patterns, be more strict in accepting loop closures
|
||||
'Rtabmap/DetectionRate': '0' # Rate is limited by the assembling time below (1 Hz)
|
||||
}
|
||||
|
||||
# Shared parameters between different nodes
|
||||
shared_parameters={
|
||||
'frame_id':'base_link',
|
||||
'use_sim_time':use_sim_time,
|
||||
# RTAB-Map's parameters should be strings:
|
||||
'Reg/Strategy':'1',
|
||||
'Reg/Force3DoF':'true', # we are moving on a 2D flat floor
|
||||
'Mem/NotLinkedNodesKept':'false',
|
||||
'Icp/VoxelSize': '0.3',
|
||||
'Icp/MaxCorrespondenceDistance': '3', # roughly 10x voxel size
|
||||
'Icp/PointToPlaneGroundNormalsUp': '0.9',
|
||||
'Icp/RangeMin': '0.5',
|
||||
'Icp/MaxTranslation': '2'
|
||||
}
|
||||
|
||||
remappings=[
|
||||
('/tf', 'tf'),
|
||||
('/tf_static', 'tf_static'),
|
||||
('odom', 'icp_odom'),
|
||||
('rgb/image', 'sensors/camera_0/color/image'),
|
||||
('rgb/camera_info', 'sensors/camera_0/color/camera_info'),
|
||||
('depth/image', 'sensors/camera_0/depth/image')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='false', choices=['true', 'false'],
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'robot_ns', default_value='a200_0000',
|
||||
description='Robot namespace.'),
|
||||
|
||||
# Nodes to launch
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}],
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_odom', executable='icp_odometry', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[icp_odom_parameters, shared_parameters],
|
||||
remappings=remappings + [('scan_cloud', 'sensors/lidar3d_0/points')],
|
||||
arguments=["--ros-args", "--log-level", 'warn']),
|
||||
|
||||
#Assemble scans
|
||||
Node(
|
||||
package='rtabmap_util', executable='point_cloud_assembler', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[{'assembling_time': 1.0, 'range_min': 0.5, 'fixed_frame_id': "", 'use_sim_time':use_sim_time, 'sync_queue_size': 30, 'topic_queue_size':30}],
|
||||
remappings=remappings + [('cloud', 'sensors/lidar3d_0/points')]),
|
||||
|
||||
# SLAM Mode:
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[rtabmap_parameters, shared_parameters],
|
||||
remappings=remappings + [('scan_cloud', 'assembled_cloud')],
|
||||
arguments=['-d']),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
namespace=robot_ns,
|
||||
parameters=[rtabmap_parameters, shared_parameters],
|
||||
remappings=remappings + [('scan_cloud', 'sensors/lidar3d_0/points')]),
|
||||
])
|
||||
@@ -0,0 +1,280 @@
|
||||
#
|
||||
# Requirements:
|
||||
# * Isaac simulator
|
||||
# * isaac_ros_image_proc
|
||||
# * isaac_ros_stereo_image_proc
|
||||
# * nav2_bringup
|
||||
# * isaac_ros_visual_slam (optional, for vo:=isaac)
|
||||
#
|
||||
# 1. Launch Isaac Simulator
|
||||
#
|
||||
# 2. Open Isaac Examples -> ROS2 -> Navigation -> Carter Navigation (or iw.hub Navigation, for more visual features)
|
||||
#
|
||||
# 3. Enable front stereo right camera:
|
||||
# In the Stage tab, open World->Nova_Carter_ROS->front_hawk->right_camera_render_product,
|
||||
# then under Property->Isaac Create Render Product Node->Inputs, check "Enabled". To make
|
||||
# simulation faster, set height=600 and width=960. Do the same for the front stereo left camera.
|
||||
#
|
||||
# 4. Make sure that after you click on Play button in the simulator, you can see these topics:
|
||||
# $ ros2 topic list
|
||||
# /front_stereo_camera/left/camera_info
|
||||
# /front_stereo_camera/left/image_raw
|
||||
# /front_stereo_camera/left/image_raw/nitros_bridge
|
||||
# /front_stereo_camera/right/camera_info
|
||||
# /front_stereo_camera/right/image_raw
|
||||
# /front_stereo_camera/right/image_raw/nitros_bridge
|
||||
# /front_stereo_imu/imu
|
||||
#
|
||||
# 5. Launch the example:
|
||||
# $ ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py
|
||||
#
|
||||
# 6. You should be able to send goals in RVIZ to move the robot, or use:
|
||||
# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard
|
||||
#
|
||||
# === Advanced ===
|
||||
# With this launch file, we can also experiment with visual odometry with/without disparity computed on GPU.
|
||||
#
|
||||
# A. Use RTAB-Map's Visual Odometry:
|
||||
# $ ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py vo:=rtabmap stereo:=true
|
||||
# $ ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py vo:=rtabmap stereo:=false
|
||||
#
|
||||
# B. Use Isaac Visual Odometry:
|
||||
# We should disable wheel odometry TF publishing in the simulator to make it work. To
|
||||
# do so, in the Stage tab, open World->Nova_Carter_ROS->transform_tree_odometry->ros2_publish_raw_transform_tree,
|
||||
# then under Property->ROS2Publish Raw Transform Tree Node->Inputs, change topicName from "tf" to "tf_odom_ignored".
|
||||
# $ ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py vo:=isaac stereo:=true
|
||||
# $ ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py vo:=isaac stereo:=false
|
||||
#
|
||||
#
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch_ros.actions import ComposableNodeContainer
|
||||
from launch_ros.descriptions import ComposableNode
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
# Directories
|
||||
pkg_nav2_bringup = get_package_share_directory(
|
||||
'nav2_bringup')
|
||||
pkg_rtabmap_demos = get_package_share_directory(
|
||||
'rtabmap_demos')
|
||||
|
||||
# Paths
|
||||
nav2_launch = PathJoinSubstitution(
|
||||
[pkg_nav2_bringup, 'launch', 'navigation_launch.py'])
|
||||
nav2_vo_params = PathJoinSubstitution(
|
||||
[pkg_rtabmap_demos, 'params', 'isaac_vslam_nav2_params.yaml'])
|
||||
nav2_params = PathJoinSubstitution(
|
||||
[pkg_rtabmap_demos, 'params', 'isaac_nav2_params.yaml'])
|
||||
rviz_launch = PathJoinSubstitution(
|
||||
[pkg_nav2_bringup, 'launch', 'rviz_launch.py'])
|
||||
rtabmap_launch = PathJoinSubstitution(
|
||||
[pkg_rtabmap_demos, 'launch', 'isaac', 'isaac_vslam.launch.py'])
|
||||
|
||||
vo = LaunchConfiguration('vo').perform(context)
|
||||
image_width = int(LaunchConfiguration('image_width').perform(context))
|
||||
image_height = int(LaunchConfiguration('image_height').perform(context))
|
||||
|
||||
left_resize_node = ComposableNode(
|
||||
name='left_resize_node',
|
||||
package='isaac_ros_image_proc',
|
||||
plugin='nvidia::isaac_ros::image_proc::ResizeNode',
|
||||
parameters=[{
|
||||
'use_sim_time': True,
|
||||
'output_width': image_width,
|
||||
'output_height': image_height,
|
||||
}],
|
||||
namespace="front_stereo_camera",
|
||||
remappings=[
|
||||
('image', 'left/image_raw'),
|
||||
('camera_info', 'left/camera_info'),
|
||||
('resize/image', 'left/image_resize'),
|
||||
('resize/camera_info', 'left/camera_info_resize')
|
||||
]
|
||||
)
|
||||
|
||||
right_resize_node = ComposableNode(
|
||||
name='right_resize_node',
|
||||
package='isaac_ros_image_proc',
|
||||
plugin='nvidia::isaac_ros::image_proc::ResizeNode',
|
||||
parameters=[{
|
||||
'use_sim_time': True,
|
||||
'output_width': image_width,
|
||||
'output_height': image_height,
|
||||
}],
|
||||
namespace="front_stereo_camera",
|
||||
remappings=[
|
||||
('image', 'right/image_raw'),
|
||||
('camera_info', 'right/camera_info'),
|
||||
('resize/image', 'right/image_resize'),
|
||||
('resize/camera_info', 'right/camera_info_resize')
|
||||
]
|
||||
)
|
||||
|
||||
left_rectify_node = ComposableNode(
|
||||
name='left_rectify_node',
|
||||
package='isaac_ros_image_proc',
|
||||
plugin='nvidia::isaac_ros::image_proc::RectifyNode',
|
||||
parameters=[{
|
||||
'use_sim_time': True,
|
||||
'output_width': image_width,
|
||||
'output_height': image_height,
|
||||
}],
|
||||
namespace="front_stereo_camera",
|
||||
remappings=[
|
||||
('image_raw', 'left/image_resize'),
|
||||
('camera_info', 'left/camera_info_resize'),
|
||||
('image_rect', 'left/image_rect'),
|
||||
('camera_info_rect', 'left/camera_info_rect')
|
||||
]
|
||||
)
|
||||
|
||||
right_rectify_node = ComposableNode(
|
||||
name='right_rectify_node',
|
||||
package='isaac_ros_image_proc',
|
||||
plugin='nvidia::isaac_ros::image_proc::RectifyNode',
|
||||
parameters=[{
|
||||
'use_sim_time': True,
|
||||
'output_width': image_width,
|
||||
'output_height': image_height,
|
||||
}],
|
||||
namespace="front_stereo_camera",
|
||||
remappings=[
|
||||
('image_raw', 'right/image_resize'),
|
||||
('camera_info', 'right/camera_info_resize'),
|
||||
('image_rect', 'right/image_rect'),
|
||||
('camera_info_rect', 'right/camera_info_rect')
|
||||
]
|
||||
)
|
||||
|
||||
disparity_node = ComposableNode(
|
||||
name='disparity_node',
|
||||
package='isaac_ros_stereo_image_proc',
|
||||
plugin='nvidia::isaac_ros::stereo_image_proc::DisparityNode',
|
||||
parameters=[{
|
||||
'use_sim_time': True,
|
||||
'backends': 'CUDA',
|
||||
'max_disparity': 64.0
|
||||
}],
|
||||
namespace="front_stereo_camera",
|
||||
remappings=[
|
||||
('left/camera_info', 'left/camera_info_rect'),
|
||||
('right/camera_info', 'right/camera_info_rect'),
|
||||
],
|
||||
)
|
||||
|
||||
disparity_to_depth_node = ComposableNode(
|
||||
name='disparity_to_depth_node',
|
||||
package='isaac_ros_stereo_image_proc',
|
||||
plugin='nvidia::isaac_ros::stereo_image_proc::DisparityToDepthNode',
|
||||
parameters=[{
|
||||
'use_sim_time': True,
|
||||
}],
|
||||
namespace="front_stereo_camera"
|
||||
)
|
||||
|
||||
stereo_img_proc_container = ComposableNodeContainer(
|
||||
name='stereo_img_proc_container',
|
||||
package='rclcpp_components',
|
||||
namespace="front_stereo_camera",
|
||||
executable='component_container_mt',
|
||||
composable_node_descriptions=[
|
||||
left_resize_node,
|
||||
right_resize_node,
|
||||
left_rectify_node,
|
||||
right_rectify_node,
|
||||
disparity_node,
|
||||
disparity_to_depth_node
|
||||
],
|
||||
output='screen',
|
||||
arguments=['--ros-args', '--log-level', 'info',
|
||||
'--log-level', 'color_format_convert:=info',
|
||||
'--log-level', 'NitrosImage:=info',
|
||||
'--log-level', 'NitrosNode:=info'
|
||||
],
|
||||
)
|
||||
|
||||
nav2_args = [('use_sim_time', 'true')]
|
||||
if vo == 'rtabmap':
|
||||
# We need to change the base odom frame to vo
|
||||
nav2_args.append(('params_file', nav2_vo_params))
|
||||
else:
|
||||
# Use custom version with higher velocities
|
||||
nav2_args.append(('params_file', nav2_params))
|
||||
nav2 = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([nav2_launch]),
|
||||
launch_arguments=nav2_args
|
||||
)
|
||||
|
||||
rviz = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([rviz_launch])
|
||||
)
|
||||
|
||||
rtabmap = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([rtabmap_launch]),
|
||||
launch_arguments=[
|
||||
('rtabmap_viz', LaunchConfiguration('rtabmap_viz')),
|
||||
('localization', LaunchConfiguration('localization')),
|
||||
('use_sim_time', 'true'),
|
||||
('stereo_camera_namespace', 'front_stereo_camera'),
|
||||
('enable_vo', str(vo == 'rtabmap')),
|
||||
('stereo', LaunchConfiguration('stereo'))
|
||||
]
|
||||
)
|
||||
|
||||
# Add actions
|
||||
actions = [rtabmap, nav2, rviz, stereo_img_proc_container]
|
||||
|
||||
if vo == 'isaac':
|
||||
isaac_visual_slam_node = ComposableNode(
|
||||
name='visual_slam_node',
|
||||
package='isaac_ros_visual_slam',
|
||||
plugin='nvidia::isaac_ros::visual_slam::VisualSlamNode',
|
||||
remappings=[('visual_slam/image_0', 'front_stereo_camera/left/image_rect'),
|
||||
('visual_slam/camera_info_0', 'front_stereo_camera/left/camera_info_rect'),
|
||||
('visual_slam/image_1', 'front_stereo_camera/right/image_rect'),
|
||||
('visual_slam/camera_info_1', 'front_stereo_camera/right/camera_info_rect')],
|
||||
parameters=[{
|
||||
'use_sim_time': True,
|
||||
'enable_image_denoising': True,
|
||||
'enable_planar_mode': True,
|
||||
'rectified_images': True,
|
||||
'publish_map_to_odom_tf': False,
|
||||
'odom_frame': 'odom',
|
||||
'enable_slam_visualization': True,
|
||||
'enable_observations_view': True,
|
||||
'enable_landmarks_view': True}]
|
||||
)
|
||||
|
||||
isaac_vslam_container = ComposableNodeContainer(
|
||||
name='isaac_visual_slam_container',
|
||||
namespace='',
|
||||
package='rclcpp_components',
|
||||
executable='component_container',
|
||||
composable_node_descriptions=[isaac_visual_slam_node],
|
||||
output='screen',
|
||||
)
|
||||
actions.append(isaac_vslam_container)
|
||||
|
||||
return actions
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument('rtabmap_viz', default_value='true',
|
||||
choices=['true', 'false'], description='Start rtabmap_viz.'),
|
||||
DeclareLaunchArgument('localization', default_value='false',
|
||||
choices=['true', 'false'], description='Start rtabmap in localization mode (a map should have been already created).'),
|
||||
DeclareLaunchArgument('vo', default_value='none',
|
||||
choices=['none', 'rtabmap', 'isaac'], description='Enable visual odometry using one of the approach. None means only wheel odometry is used. If you set this to "isaac", make sure to disable odom -> base_link if it exists, because isaac will publish on same TF!'),
|
||||
DeclareLaunchArgument('stereo', default_value='true',
|
||||
choices=['true', 'false'], description='Use stereo images as input instead of left+depth images.'),
|
||||
DeclareLaunchArgument('image_width', default_value='960',
|
||||
description='Resize input images.'),
|
||||
DeclareLaunchArgument('image_height', default_value='600',
|
||||
description='Resize input images.'),
|
||||
OpaqueFunction(function=launch_setup)
|
||||
])
|
||||
@@ -0,0 +1,139 @@
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
localization = LaunchConfiguration('localization')
|
||||
localization_value = localization.perform(context)
|
||||
localization_value = localization_value == 'True' or localization_value == 'true'
|
||||
enable_vo = LaunchConfiguration('enable_vo')
|
||||
enable_vo_value = enable_vo.perform(context)
|
||||
enable_vo_value = enable_vo_value == 'True' or enable_vo_value == 'true'
|
||||
stereo = LaunchConfiguration('stereo')
|
||||
stereo_value = stereo.perform(context)
|
||||
stereo_value = stereo_value == 'True' or stereo_value == 'true'
|
||||
rtabmap_viz = LaunchConfiguration('rtabmap_viz')
|
||||
stereo_ns = LaunchConfiguration('stereo_camera_namespace').perform(context)
|
||||
|
||||
parameters={
|
||||
'frame_id':'base_link',
|
||||
'use_sim_time': use_sim_time,
|
||||
'subscribe_rgbd': True,
|
||||
'subscribe_odom': enable_vo,
|
||||
'subscribe_odom_info': enable_vo,
|
||||
'approx_sync': False,
|
||||
'use_action_for_goal':True,
|
||||
'Reg/Force3DoF':'true',
|
||||
'Vis/MinDepth': '0.2',
|
||||
'GFTT/MinDistance': '5',
|
||||
'GFTT/QualityLevel': '0.00001',
|
||||
'Grid/RayTracing':'true', # Fill empty space
|
||||
'Grid/3D':'false', # Use 2D occupancy
|
||||
'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles
|
||||
'Grid/MaxGroundHeight':'0.15', # All points above 5 cm are obstacles
|
||||
'Grid/MaxObstacleHeight':'0.5', # All points over 0.5 meter are ignored
|
||||
'Grid/RangeMin':'0.2', # Ignore invalid points close to camera
|
||||
'Grid/NoiseFilteringMinNeighbors':'8', # Default stereo is quite noisy, enable noise filter
|
||||
'Grid/NoiseFilteringRadius':'0.1', # Default stereo is quite noisy, enable noise filter
|
||||
'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
|
||||
}
|
||||
if enable_vo_value:
|
||||
parameters['guess_frame_id'] = 'odom'
|
||||
else:
|
||||
parameters['odom_frame_id'] = 'odom'
|
||||
|
||||
arguments = []
|
||||
if localization_value:
|
||||
parameters['Mem/IncrementalMemory'] = 'True'
|
||||
parameters['Mem/InitWMWithAllNodes'] = 'True'
|
||||
else:
|
||||
arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db)
|
||||
|
||||
remappings=[('rgbd_image', '/'+stereo_ns+'/rgbd_image'),
|
||||
('map', '/map')]
|
||||
vo_node_prefix = 'rgbd'
|
||||
if stereo_value:
|
||||
vo_node_prefix = 'stereo'
|
||||
|
||||
return [
|
||||
# Sync image data together
|
||||
Node(
|
||||
condition=UnlessCondition(stereo),
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
namespace=stereo_ns,
|
||||
parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}],
|
||||
remappings=[
|
||||
('rgb/image', 'left/image_rect'),
|
||||
('rgb/camera_info', 'left/camera_info_rect'),
|
||||
('depth/image', 'depth')]),
|
||||
|
||||
Node(
|
||||
condition=IfCondition(stereo),
|
||||
package='rtabmap_sync', executable='stereo_sync', output='screen',
|
||||
namespace=stereo_ns,
|
||||
parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}],
|
||||
remappings=[
|
||||
('left/image_rect', 'left/image_rect'),
|
||||
('left/camera_info', 'left/camera_info_rect'),
|
||||
('right/image_rect', 'right/image_rect'),
|
||||
('right/camera_info', 'right/camera_info_rect')]),
|
||||
|
||||
Node(
|
||||
condition=IfCondition(enable_vo),
|
||||
package='rtabmap_odom', executable=vo_node_prefix+'_odometry', output='screen',
|
||||
namespace='rtabmap',
|
||||
parameters=[parameters, {'odom_frame_id': 'vo'}],
|
||||
remappings=remappings),
|
||||
|
||||
# VSLAM:
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
namespace='rtabmap',
|
||||
parameters=[parameters],
|
||||
remappings=remappings,
|
||||
arguments=arguments),
|
||||
|
||||
# Visualization:
|
||||
Node(
|
||||
condition=IfCondition(rtabmap_viz),
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
namespace='rtabmap',
|
||||
parameters=[parameters],
|
||||
remappings=remappings),
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='true',
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Launch in localization mode.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'enable_vo', default_value='false',
|
||||
description='Enable RTAB-Map\'s visual odometry.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'rtabmap_viz', default_value='true',
|
||||
description='Launch rtabmap_viz for visualization.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'stereo', default_value='false',
|
||||
description='Use stereo images as input instead of left+depth images.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'stereo_camera_namespace', default_value='front_stereo_camera',
|
||||
description='Namespace of the stereo camera.'),
|
||||
|
||||
OpaqueFunction(function=launch_setup)
|
||||
])
|
||||
@@ -0,0 +1,115 @@
|
||||
# Requirements:
|
||||
# Download one or more rosbags:
|
||||
# * map1.db3: https://drive.google.com/file/d/1XajzWm0u1Tk7m7x63ybcKVMXj80r5P6r/view?usp=drive_link
|
||||
# * map2.db3: https://drive.google.com/file/d/1_FxEalE2O-DQKq2tRpLIpDn5Mbvu0jZc/view?usp=drive_link
|
||||
# * map3.db3: https://drive.google.com/file/d/1dJzMOoRPA28gQZUIWCeAa08Qn4wG9oRw/view?usp=drive_link
|
||||
# * map4.db3: https://drive.google.com/file/d/19Y6yye0ndIIwdhEWMwTdoiSy9WlKS44c/view?usp=drive_link
|
||||
# * map5.db3: https://drive.google.com/file/d/1zCx4Q4SftPplQtW1xeG-W3OkbTxwd5GD/view?usp=drive_link
|
||||
#
|
||||
# Example:
|
||||
#
|
||||
# SLAM:
|
||||
# $ rm ~/.ros/rtabmap.db
|
||||
# $ ros2 launch rtabmap_demos multisession_mapping_demo.launch.py
|
||||
#
|
||||
# Rosbag:
|
||||
# $ ros2 bag play map1.db3 --clock
|
||||
# when done, you can play the next bag(s):
|
||||
# $ ros2 bag play map2.db3 --clock
|
||||
# $ ros2 bag play map3.db3 --clock
|
||||
# $ ros2 bag play map4.db3 --clock
|
||||
# $ ros2 bag play map5.db3 --clock
|
||||
#
|
||||
# Refer to this paper for more info: https://arxiv.org/abs/2407.15305
|
||||
#
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.conditions import IfCondition
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.actions import SetParameter
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
parameters={
|
||||
'frame_id':'base_footprint',
|
||||
'odom_frame_id':'odom',
|
||||
'odom_tf_linear_variance':0.001,
|
||||
'odom_tf_angular_variance':0.001,
|
||||
'subscribe_rgbd':True,
|
||||
'subscribe_scan':True,
|
||||
'approx_sync':True,
|
||||
'sync_queue_size': 10,
|
||||
# RTAB-Map's internal parameters should be strings
|
||||
'RGBD/NeighborLinkRefining': 'false',
|
||||
'RGBD/ProximityBySpace': 'false', # Referred paper did only global loop closure detection
|
||||
'RGBD/OptimizeFromGraphEnd': 'true',
|
||||
'Reg/Strategy': '1',
|
||||
'Icp/Iterations': '30',
|
||||
'Icp/VoxelSize': '0',
|
||||
'Vis/MinInliers': '12',
|
||||
'Vis/MaxDepth': '0',
|
||||
'RGBD/AngularUpdate': '0.01',
|
||||
'RGBD/LinearUpdate': '0.01',
|
||||
'Rtabmap/TimeThr': '700',
|
||||
'Mem/RehearsalSimilarity': '0.30', # Referred paper used 0.45 with SURF, here with SIFT, we will use 0.3
|
||||
'Kp/TfIdfLikelihoodUsed': 'false',
|
||||
'Bayes/FullPredictionUpdate': 'true',
|
||||
'Kp/DetectorStrategy': '1', # Referred paper used SURF (0), here use SIFT as it is available with opencv binaries
|
||||
'Vis/FeatureType': '1', # Referred paper used SURF (0), here use SIFT as it is available with opencv binaries
|
||||
'Kp/MaxFeatures': '400',
|
||||
'Reg/Force3DoF': 'true',
|
||||
'RGBD/OptimizeMaxError': '10',
|
||||
'Optimizer/Strategy': '2', # Referred paper used TORO (0), latest version recommends GTSAM (2)
|
||||
'Optimizer/Iterations': '100',
|
||||
'Kp/IncrementalFlann': 'false', # Referred paper didn't use incremental FLANN
|
||||
'Icp/MaxTranslation': '0.5',
|
||||
}
|
||||
|
||||
remappings=[
|
||||
('rgb/image', '/data_throttled_image'),
|
||||
('depth/image', '/data_throttled_image_depth'),
|
||||
('rgb/camera_info', '/data_throttled_camera_info'),
|
||||
('scan', '/base_scan')]
|
||||
|
||||
config_rviz = os.path.join(
|
||||
get_package_share_directory('rtabmap_demos'), 'config', 'demo_robot_mapping.rviz'
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument('rtabmap_viz', default_value='true', description='Launch RTAB-Map UI (optional).'),
|
||||
DeclareLaunchArgument('rviz', default_value='false', description='Launch RVIZ (optional).'),
|
||||
DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'),
|
||||
|
||||
SetParameter(name='use_sim_time', value=True),
|
||||
|
||||
# Nodes to launch
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
parameters=[parameters,
|
||||
{'rgb_image_transport':'compressed',
|
||||
'depth_image_transport':'compressedDepth',
|
||||
'approx_sync_max_interval': 0.02}],
|
||||
remappings=remappings),
|
||||
|
||||
# SLAM node:
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=remappings),
|
||||
|
||||
# Visualization:
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
condition=IfCondition(LaunchConfiguration("rtabmap_viz")),
|
||||
parameters=[parameters],
|
||||
remappings=remappings),
|
||||
Node(
|
||||
package='rviz2', executable='rviz2', name="rviz2", output='screen',
|
||||
condition=IfCondition(LaunchConfiguration("rviz")),
|
||||
arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]),
|
||||
])
|
||||
@@ -0,0 +1,113 @@
|
||||
# Requirements:
|
||||
# Download rosbag:
|
||||
# * demo_mapping.db3: https://drive.google.com/file/d/1v9qJ2U7GlYhqBJr7OQHWbDSCfgiVaLWb/view?usp=drive_link
|
||||
#
|
||||
# Example:
|
||||
#
|
||||
# SLAM:
|
||||
# $ ros2 launch rtabmap_demos robot_mapping_demo.launch.py rviz:=true rtabmap_viz:=true
|
||||
#
|
||||
# Rosbag:
|
||||
# $ ros2 bag play demo_mapping.db3 --clock
|
||||
#
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.actions import SetParameter
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
localization = LaunchConfiguration('localization')
|
||||
|
||||
parameters={
|
||||
'frame_id':'base_footprint',
|
||||
'odom_frame_id':'odom',
|
||||
'odom_tf_linear_variance':0.001,
|
||||
'odom_tf_angular_variance':0.001,
|
||||
'subscribe_rgbd':True,
|
||||
'subscribe_scan':True,
|
||||
'approx_sync':True,
|
||||
'sync_queue_size': 10,
|
||||
# RTAB-Map's internal parameters should be strings
|
||||
'RGBD/NeighborLinkRefining': 'true', # Do odometry correction with consecutive laser scans
|
||||
'RGBD/ProximityBySpace': 'true', # Local loop closure detection (using estimated position) with locations in WM
|
||||
'RGBD/ProximityByTime': 'false', # Local loop closure detection with locations in STM
|
||||
'RGBD/ProximityPathMaxNeighbors': '10', # Do also proximity detection by space by merging close scans together.
|
||||
'Reg/Strategy': '1', # 0=Visual, 1=ICP, 2=Visual+ICP
|
||||
'Vis/MinInliers': '12', # 3D visual words minimum inliers to accept loop closure
|
||||
'RGBD/OptimizeFromGraphEnd': 'false', # Optimize graph from initial node so /map -> /odom transform will be generated
|
||||
'RGBD/OptimizeMaxError': '4', # Reject any loop closure causing large errors (>3x link's covariance) in the map
|
||||
'Reg/Force3DoF': 'true', # 2D SLAM
|
||||
'Grid/FromDepth': 'false', # Create 2D occupancy grid from laser scan
|
||||
'Mem/STMSize': '30', # increased to 30 to avoid adding too many loop closures on just seen locations
|
||||
'RGBD/LocalRadius': '5', # limit length of proximity detections
|
||||
'Icp/CorrespondenceRatio': '0.2', # minimum scan overlap to accept loop closure
|
||||
'Icp/PM': 'false',
|
||||
'Icp/PointToPlane': 'false',
|
||||
'Icp/MaxCorrespondenceDistance': '0.15',
|
||||
'Icp/VoxelSize': '0.05'
|
||||
}
|
||||
|
||||
remappings=[
|
||||
('rgb/image', '/camera/color/image_raw'),
|
||||
('depth/image', '/camera/depth/image_raw'),
|
||||
('rgb/camera_info', '/camera/color/camera_info'),
|
||||
('scan', '/scan')]
|
||||
|
||||
config_rviz = os.path.join(
|
||||
get_package_share_directory('rtabmap_demos'), 'config', 'demo_robot_mapping.rviz'
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument('rtabmap_viz', default_value='false', description='Launch RTAB-Map UI (optional).'),
|
||||
DeclareLaunchArgument('rviz', default_value='true', description='Launch RVIZ (optional).'),
|
||||
DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'),
|
||||
DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'),
|
||||
|
||||
SetParameter(name='use_sim_time', value=False),
|
||||
|
||||
# Nodes to launch
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
parameters=[parameters,
|
||||
{
|
||||
# 'rgb_image_transport':'compressed',
|
||||
# 'depth_image_transport':'compressedDepth',
|
||||
'approx_sync_max_interval': 0.1}],
|
||||
remappings=remappings),
|
||||
|
||||
# SLAM mode:
|
||||
Node(
|
||||
condition=UnlessCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=remappings,
|
||||
arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db)
|
||||
|
||||
# Localization mode:
|
||||
Node(
|
||||
condition=IfCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[parameters,
|
||||
{'Mem/IncrementalMemory':'False',
|
||||
'Mem/InitWMWithAllNodes':'True'}],
|
||||
remappings=remappings),
|
||||
|
||||
# Visualization:
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
condition=IfCondition(LaunchConfiguration("rtabmap_viz")),
|
||||
parameters=[parameters],
|
||||
remappings=remappings),
|
||||
Node(
|
||||
package='rviz2', executable='rviz2', name="rviz2", output='screen',
|
||||
condition=IfCondition(LaunchConfiguration("rviz")),
|
||||
arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]),
|
||||
])
|
||||
@@ -0,0 +1,155 @@
|
||||
# Requirements:
|
||||
# Download one or both rosbags:
|
||||
# * stereo_outdoorA.db3: https://drive.google.com/file/d/1O7mCXg_sw4tZY1S88a-n96O6OulmqvqI/view?usp=drive_link
|
||||
# * stereo_outdoorB.db3: https://drive.google.com/file/d/1mSu7418Fkbe-hIz2-3Mi936PrWuD2un_/view?usp=drive_link
|
||||
#
|
||||
# Example:
|
||||
#
|
||||
# SLAM:
|
||||
# $ ros2 launch rtabmap_demos stereo_outdoor_demo.launch.py rviz:=true rtabmap_viz:=true
|
||||
#
|
||||
# Rosbag:
|
||||
# $ ros2 bag play stereo_outdoorA.db3 --clock
|
||||
# when done, you can play the secon bag:
|
||||
# $ ros2 bag play stereo_outdoorB.db3 --clock
|
||||
#
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch_ros.actions import Node, SetParameter, SetRemap
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
pkg_stereo_image_proc = get_package_share_directory(
|
||||
'stereo_image_proc')
|
||||
|
||||
# Paths
|
||||
stereo_image_proc_launch = PathJoinSubstitution(
|
||||
[pkg_stereo_image_proc, 'launch', 'stereo_image_proc.launch.py'])
|
||||
|
||||
localization = LaunchConfiguration('localization')
|
||||
|
||||
parameters={
|
||||
'frame_id':'base_footprint',
|
||||
'subscribe_rgbd':True,
|
||||
'approx_sync':False, # odom is generated from images, so we can exactly sync all inputs
|
||||
'map_negative_poses_ignored':True,
|
||||
'subscribe_odom_info': True,
|
||||
# RTAB-Map's internal parameters should be strings
|
||||
'OdomF2M/MaxSize': '1000',
|
||||
'GFTT/MinDistance': '10',
|
||||
'GFTT/QualityLevel': '0.00001',
|
||||
#'Kp/DetectorStrategy': '6', # Uncommment to match ros1 noetic results, but opencv should be built with xfeatures2d
|
||||
#'Vis/FeatureType': '6' # Uncommment to match ros1 noetic results, but opencv should be built with xfeatures2d
|
||||
}
|
||||
|
||||
remappings=[
|
||||
('rgbd_image', '/stereo_camera/rgbd_image'),
|
||||
('odom', '/vo')]
|
||||
|
||||
config_rviz = os.path.join(
|
||||
get_package_share_directory('rtabmap_demos'), 'config', 'demo_robot_mapping.rviz'
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument('rtabmap_viz', default_value='false', description='Launch RTAB-Map UI (optional).'),
|
||||
DeclareLaunchArgument('rviz', default_value='true', description='Launch RVIZ (optional).'),
|
||||
DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'),
|
||||
DeclareLaunchArgument('rviz_cfg', default_value=config_rviz, description='Configuration path of rviz2.'),
|
||||
|
||||
SetParameter(name='use_sim_time', value=True),
|
||||
|
||||
# Nodes to launch
|
||||
|
||||
# Uncompress images for stereo_image_rect and remap to expected names from stereo_image_proc
|
||||
Node(
|
||||
package='image_transport', executable='republish', name='republish_left', output='screen',
|
||||
namespace='stereo_camera',
|
||||
arguments=['compressed', 'raw'],
|
||||
remappings=[('in/compressed', 'left/image_raw_throttle/compressed'),
|
||||
('out', 'left/image_raw')]),
|
||||
Node(
|
||||
package='image_transport', executable='republish', name='republish_right', output='screen',
|
||||
namespace='stereo_camera',
|
||||
arguments=['compressed', 'raw'],
|
||||
remappings=[('in/compressed', 'right/image_raw_throttle/compressed'),
|
||||
('out', 'right/image_raw')]),
|
||||
|
||||
# Run the ROS package stereo_image_proc for image rectification
|
||||
GroupAction(
|
||||
actions=[
|
||||
|
||||
SetRemap(src='camera_info',dst='camera_info_throttle'),
|
||||
SetRemap(src='camera_info',dst='camera_info_throttle'),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([stereo_image_proc_launch]),
|
||||
launch_arguments=[
|
||||
('left_namespace', 'stereo_camera/left'),
|
||||
('right_namespace', 'stereo_camera/right'),
|
||||
('disparity_range', '128'),
|
||||
]
|
||||
),
|
||||
]
|
||||
),
|
||||
|
||||
# Synchronize stereo data together in a single topic
|
||||
# Issue: stereo_img_proc doesn't produce color and
|
||||
# grayscale images exactly the same (there is a small
|
||||
# vertical shift with color), we should use grayscale for
|
||||
# left and right images to get similar results than on ros1 noetic.
|
||||
Node(
|
||||
package='rtabmap_sync', executable='stereo_sync', output='screen',
|
||||
namespace='stereo_camera',
|
||||
remappings=[
|
||||
('left/image_rect', 'left/image_rect'),
|
||||
('right/image_rect', 'right/image_rect'),
|
||||
('left/camera_info', 'left/camera_info_throttle'),
|
||||
('right/camera_info', 'right/camera_info_throttle')]),
|
||||
|
||||
# Visual odometry
|
||||
Node(
|
||||
package='rtabmap_odom', executable='stereo_odometry', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=remappings),
|
||||
|
||||
# SLAM mode:
|
||||
Node(
|
||||
condition=UnlessCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=remappings,
|
||||
arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db)
|
||||
|
||||
# Localization mode:
|
||||
Node(
|
||||
condition=IfCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[parameters,
|
||||
{'Mem/IncrementalMemory':'False',
|
||||
'Mem/InitWMWithAllNodes':'True'}],
|
||||
remappings=remappings),
|
||||
|
||||
# Visualization:
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
condition=IfCondition(LaunchConfiguration("rtabmap_viz")),
|
||||
parameters=[parameters],
|
||||
remappings=remappings),
|
||||
Node(
|
||||
package='rviz2', executable='rviz2', name="rviz2", output='screen',
|
||||
condition=IfCondition(LaunchConfiguration("rviz")),
|
||||
arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]),
|
||||
])
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,101 @@
|
||||
# Example:
|
||||
#
|
||||
# Bringup turtlebot3:
|
||||
# $ export TURTLEBOT3_MODEL=waffle
|
||||
# $ export LDS_MODEL=LDS-01
|
||||
# $ ros2 launch turtlebot3_bringup robot.launch.py
|
||||
#
|
||||
# SLAM:
|
||||
# $ ros2 launch rtabmap_demos turtlebot3_rgbd.launch.py
|
||||
#
|
||||
# Navigation (install nav2_bringup package):
|
||||
# $ ros2 launch nav2_bringup navigation_launch.py
|
||||
# $ ros2 launch nav2_bringup rviz_launch.py
|
||||
#
|
||||
# Teleop:
|
||||
# $ ros2 run turtlebot3_teleop teleop_keyboard
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
localization = LaunchConfiguration('localization')
|
||||
|
||||
parameters={
|
||||
'frame_id':'base_footprint',
|
||||
'use_sim_time':use_sim_time,
|
||||
'subscribe_depth':True,
|
||||
'use_action_for_goal':True,
|
||||
'Reg/Force3DoF':'true',
|
||||
'Grid/RayTracing':'true', # Fill empty space
|
||||
'Grid/3D':'false', # Use 2D occupancy
|
||||
'Grid/RangeMax':'3',
|
||||
'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles
|
||||
'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles
|
||||
'Grid/MaxObstacleHeight':'0.4', # All points over 1 meter are ignored
|
||||
'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
|
||||
}
|
||||
|
||||
remappings=[
|
||||
('rgb/image', '/camera/image_raw'),
|
||||
('rgb/camera_info', '/camera/camera_info'),
|
||||
('depth/image', '/camera/depth/image_raw')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='true',
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Launch in localization mode.'),
|
||||
|
||||
# Nodes to launch
|
||||
|
||||
# SLAM mode:
|
||||
Node(
|
||||
condition=UnlessCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=remappings,
|
||||
arguments=['-d']), # This will delete the previous database (~/.ros/rtabmap.db)
|
||||
|
||||
# Localization mode:
|
||||
Node(
|
||||
condition=IfCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[parameters,
|
||||
{'Mem/IncrementalMemory':'False',
|
||||
'Mem/InitWMWithAllNodes':'True'}],
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=remappings),
|
||||
|
||||
# Obstacle detection with the camera for nav2 local costmap.
|
||||
# First, we need to convert depth image to a point cloud.
|
||||
# Second, we segment the floor from the obstacles.
|
||||
Node(
|
||||
package='rtabmap_util', executable='point_cloud_xyz', output='screen',
|
||||
parameters=[{'decimation': 2,
|
||||
'max_depth': 3.0,
|
||||
'voxel_size': 0.02}],
|
||||
remappings=[('depth/image', '/camera/depth/image_raw'),
|
||||
('depth/camera_info', '/camera/camera_info'),
|
||||
('cloud', '/camera/cloud')]),
|
||||
Node(
|
||||
package='rtabmap_util', executable='obstacles_detection', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=[('cloud', '/camera/cloud'),
|
||||
('obstacles', '/camera/obstacles'),
|
||||
('ground', '/camera/ground')]),
|
||||
])
|
||||
@@ -0,0 +1,143 @@
|
||||
# Example:
|
||||
#
|
||||
# Bringup turtlebot3:
|
||||
# $ export TURTLEBOT3_MODEL=waffle
|
||||
# $ export LDS_MODEL=LDS-01
|
||||
# $ ros2 launch turtlebot3_bringup robot.launch.py
|
||||
#
|
||||
# SLAM:
|
||||
# $ ros2 launch rtabmap_demos turtlebot3_rgbd_fake_scan.launch.py
|
||||
#
|
||||
# Navigation (install nav2_bringup package):
|
||||
# $ ros2 launch nav2_bringup navigation_launch.py
|
||||
# $ ros2 launch nav2_bringup rviz_launch.py
|
||||
#
|
||||
# Teleop:
|
||||
# $ ros2 run turtlebot3_teleop teleop_keyboard
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
localization = LaunchConfiguration('localization')
|
||||
|
||||
parameters={
|
||||
'frame_id':'base_footprint',
|
||||
'use_sim_time':use_sim_time,
|
||||
'subscribe_rgbd':True,
|
||||
'subscribe_scan_cloud':True,
|
||||
'use_action_for_goal':True,
|
||||
'scan_cloud_is_2d': True,
|
||||
# RTAB-Map's parameters should be strings:
|
||||
'Reg/Strategy':'1',
|
||||
'Reg/Force3DoF':'true',
|
||||
'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
|
||||
}
|
||||
|
||||
remappings=[
|
||||
('rgb/image', '/camera/image_raw'),
|
||||
('rgb/camera_info', '/camera/camera_info'),
|
||||
('depth/image', '/camera/depth/image_raw'),
|
||||
('scan_cloud', 'assembled_cloud')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='false',
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Launch in localization mode.'),
|
||||
|
||||
# Nodes to launch
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}],
|
||||
remappings=remappings),
|
||||
|
||||
# Convert middle row of depth pixels to a fake laser scan
|
||||
Node(
|
||||
package='depthimage_to_laserscan', executable='depthimage_to_laserscan_node', output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time':use_sim_time,
|
||||
'range_max': 5.0
|
||||
}],
|
||||
remappings=[
|
||||
('depth', '/camera/depth/image_raw'),
|
||||
('depth_camera_info', '/camera/camera_info'),
|
||||
('scan', '/camera/scan')
|
||||
]),
|
||||
|
||||
# Just to convert the fake laser scan to PointCloud2
|
||||
Node(
|
||||
package='rtabmap_util', executable='lidar_deskewing', output='screen',
|
||||
parameters=[{'use_sim_time':use_sim_time,
|
||||
'fixed_frame_id': 'camera_link'}], # use camera frame
|
||||
remappings=[
|
||||
('input_scan', '/camera/scan')
|
||||
]),
|
||||
|
||||
# Assemble the fake laser scans using a circular buffer, then feed that cloud to rtabmap
|
||||
Node(
|
||||
package='rtabmap_util', executable='point_cloud_assembler', output='screen',
|
||||
parameters=[{'use_sim_time':use_sim_time,
|
||||
'max_clouds': 20,
|
||||
'voxel_size': 0.05,
|
||||
'wait_for_transform': 1.0,
|
||||
'linear_update': 0.3,
|
||||
'angular_update': 0.5,
|
||||
'circular_buffer': True,
|
||||
'frame_id': 'base_link'}],
|
||||
remappings=[
|
||||
('assembled_cloud', 'assembled_cloud'),
|
||||
('cloud', '/camera/scan/deskewed')
|
||||
]),
|
||||
|
||||
# SLAM Mode:
|
||||
Node(
|
||||
condition=UnlessCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
# Localization mode:
|
||||
Node(
|
||||
condition=IfCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[parameters,
|
||||
{'Mem/IncrementalMemory':'False',
|
||||
'Mem/InitWMWithAllNodes':'True'}],
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=remappings),
|
||||
|
||||
# Obstacle detection with the camera for nav2 local costmap.
|
||||
# First, we need to convert depth image to a point cloud.
|
||||
# Second, we segment the floor from the obstacles.
|
||||
Node(
|
||||
package='rtabmap_util', executable='point_cloud_xyz', output='screen',
|
||||
parameters=[{'decimation': 2,
|
||||
'max_depth': 3.0,
|
||||
'voxel_size': 0.02}],
|
||||
remappings=[('depth/image', '/camera/depth/image_raw'),
|
||||
('depth/camera_info', '/camera/camera_info'),
|
||||
('cloud', '/camera/cloud')]),
|
||||
Node(
|
||||
package='rtabmap_util', executable='obstacles_detection', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=[('cloud', '/camera/cloud'),
|
||||
('obstacles', '/camera/obstacles'),
|
||||
('ground', '/camera/ground')]),
|
||||
])
|
||||
@@ -0,0 +1,112 @@
|
||||
# Example:
|
||||
#
|
||||
# Bringup turtlebot3:
|
||||
# $ export TURTLEBOT3_MODEL=waffle
|
||||
# $ export LDS_MODEL=LDS-01
|
||||
# $ ros2 launch turtlebot3_bringup robot.launch.py
|
||||
#
|
||||
# SLAM:
|
||||
# $ ros2 launch rtabmap_demos turtlebot3_rgbd_scan.launch.py
|
||||
#
|
||||
# Navigation (install nav2_bringup package):
|
||||
# $ ros2 launch nav2_bringup navigation_launch.py
|
||||
# $ ros2 launch nav2_bringup rviz_launch.py
|
||||
#
|
||||
# Teleop:
|
||||
# $ ros2 run turtlebot3_teleop teleop_keyboard
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
localization = LaunchConfiguration('localization')
|
||||
|
||||
parameters={
|
||||
'frame_id':'base_footprint',
|
||||
'use_sim_time':use_sim_time,
|
||||
'subscribe_rgbd':True,
|
||||
'subscribe_scan':True,
|
||||
'use_action_for_goal':True,
|
||||
# RTAB-Map's parameters should be strings:
|
||||
'Reg/Strategy':'1',
|
||||
'Reg/Force3DoF':'true',
|
||||
'RGBD/NeighborLinkRefining':'True',
|
||||
'Grid/RayTracing':'true', # Fill empty space
|
||||
'Grid/3D':'false', # Use 2D occupancy
|
||||
'Grid/RangeMax':'3',
|
||||
'Grid/NormalsSegmentation':'false', # Use passthrough filter to detect obstacles
|
||||
'Grid/Sensor':'2', # Use both laser scan and camera for obstacle detection in global map
|
||||
'Grid/MaxGroundHeight':'0.05', # All points above 5 cm are obstacles
|
||||
'Grid/MaxObstacleHeight':'0.4', # All points over 1 meter are ignored
|
||||
'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself
|
||||
'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
|
||||
}
|
||||
|
||||
remappings=[
|
||||
('rgb/image', '/camera/image_raw'),
|
||||
('rgb/camera_info', '/camera/camera_info'),
|
||||
('depth/image', '/camera/depth/image_raw')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='false',
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Launch in localization mode.'),
|
||||
|
||||
# Nodes to launch
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}],
|
||||
remappings=remappings),
|
||||
|
||||
# SLAM Mode:
|
||||
Node(
|
||||
condition=UnlessCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
# Localization mode:
|
||||
Node(
|
||||
condition=IfCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[parameters,
|
||||
{'Mem/IncrementalMemory':'False',
|
||||
'Mem/InitWMWithAllNodes':'True'}],
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=remappings),
|
||||
|
||||
# Obstacle detection with the camera for nav2 local costmap.
|
||||
# First, we need to convert depth image to a point cloud.
|
||||
# Second, we segment the floor from the obstacles.
|
||||
Node(
|
||||
package='rtabmap_util', executable='point_cloud_xyz', output='screen',
|
||||
parameters=[{'decimation': 2,
|
||||
'max_depth': 3.0,
|
||||
'voxel_size': 0.02}],
|
||||
remappings=[('depth/image', '/camera/depth/image_raw'),
|
||||
('depth/camera_info', '/camera/camera_info'),
|
||||
('cloud', '/camera/cloud')]),
|
||||
Node(
|
||||
package='rtabmap_util', executable='obstacles_detection', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=[('cloud', '/camera/cloud'),
|
||||
('obstacles', '/camera/obstacles'),
|
||||
('ground', '/camera/ground')]),
|
||||
])
|
||||
@@ -0,0 +1,100 @@
|
||||
# Example:
|
||||
#
|
||||
# Bringup turtlebot3:
|
||||
# $ export TURTLEBOT3_MODEL=waffle
|
||||
# $ export LDS_MODEL=LDS-01
|
||||
# $ ros2 launch turtlebot3_bringup robot.launch.py
|
||||
#
|
||||
# SLAM:
|
||||
# $ ros2 launch rtabmap_demos turtlebot3_scan.launch.py
|
||||
#
|
||||
# Navigation (install nav2_bringup package):
|
||||
# $ ros2 launch nav2_bringup navigation_launch.py
|
||||
# $ ros2 launch nav2_bringup rviz_launch.py
|
||||
#
|
||||
# Teleop:
|
||||
# $ ros2 run turtlebot3_teleop teleop_keyboard
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
localization = LaunchConfiguration('localization').perform(context)
|
||||
localization = localization == 'True' or localization == 'true'
|
||||
icp_odometry = LaunchConfiguration('icp_odometry').perform(context)
|
||||
icp_odometry = icp_odometry == 'True' or icp_odometry == 'true'
|
||||
|
||||
parameters={
|
||||
'frame_id':'base_footprint',
|
||||
'use_sim_time':use_sim_time,
|
||||
'subscribe_depth':False,
|
||||
'subscribe_rgb':False,
|
||||
'subscribe_scan':True,
|
||||
'approx_sync':True,
|
||||
'use_action_for_goal':True,
|
||||
'Reg/Strategy':'1',
|
||||
'Reg/Force3DoF':'true',
|
||||
'RGBD/NeighborLinkRefining':'True',
|
||||
'Grid/RangeMin':'0.2', # ignore laser scan points on the robot itself
|
||||
'Optimizer/GravitySigma':'0' # Disable imu constraints (we are already in 2D)
|
||||
}
|
||||
arguments = []
|
||||
if localization:
|
||||
parameters['Mem/IncrementalMemory'] = 'False'
|
||||
parameters['Mem/InitWMWithAllNodes'] = 'True'
|
||||
else:
|
||||
arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db)
|
||||
|
||||
remappings=[
|
||||
('scan', '/scan')]
|
||||
if icp_odometry:
|
||||
remappings.append(('odom', 'icp_odom'))
|
||||
|
||||
return [
|
||||
# Nodes to launch
|
||||
|
||||
# ICP odometry (optional)
|
||||
Node(
|
||||
condition=IfCondition(LaunchConfiguration('icp_odometry')),
|
||||
package='rtabmap_odom', executable='icp_odometry', output='screen',
|
||||
parameters=[parameters,
|
||||
{'odom_frame_id':'icp_odom',
|
||||
'guess_frame_id':'odom'}],
|
||||
remappings=remappings),
|
||||
|
||||
# SLAM:
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=remappings,
|
||||
arguments=arguments),
|
||||
|
||||
# Visualization
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=remappings),
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='true',
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Launch in localization mode.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'icp_odometry', default_value='false',
|
||||
description='Launch ICP odometry on top of wheel odometry.'),
|
||||
|
||||
OpaqueFunction(function=launch_setup)
|
||||
])
|
||||
@@ -0,0 +1,117 @@
|
||||
# Requirements:
|
||||
# Install Turtlebot3 packages
|
||||
# Modify turtlebot3_waffle SDF:
|
||||
# 1) Edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf
|
||||
# 2) Add
|
||||
# <joint name="camera_rgb_optical_joint" type="fixed">
|
||||
# <parent>camera_rgb_frame</parent>
|
||||
# <child>camera_rgb_optical_frame</child>
|
||||
# <pose>0 0 0 -1.57079632679 0 -1.57079632679</pose>
|
||||
# <axis>
|
||||
# <xyz>0 0 1</xyz>
|
||||
# </axis>
|
||||
# </joint>
|
||||
# 3) Rename <link name="camera_rgb_frame"> to <link name="camera_rgb_optical_frame">
|
||||
# 4) Add <link name="camera_rgb_frame"/>
|
||||
# 5) Change <sensor name="camera" type="camera"> to <sensor name="camera" type="depth">
|
||||
# 6) Change image width/height from 1920x1080 to 640x480
|
||||
# Example:
|
||||
# $ ros2 launch rtabmap_demos turtlebot3_sim_rgbd_demo.launch.py
|
||||
#
|
||||
# Teleop:
|
||||
# $ ros2 run turtlebot3_teleop teleop_keyboard
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
import os
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
if not 'TURTLEBOT3_MODEL' in os.environ:
|
||||
os.environ['TURTLEBOT3_MODEL'] = 'waffle'
|
||||
|
||||
# Directories
|
||||
pkg_turtlebot3_gazebo = get_package_share_directory(
|
||||
'turtlebot3_gazebo')
|
||||
pkg_nav2_bringup = get_package_share_directory(
|
||||
'nav2_bringup')
|
||||
pkg_rtabmap_demos = get_package_share_directory(
|
||||
'rtabmap_demos')
|
||||
|
||||
world = LaunchConfiguration('world').perform(context)
|
||||
|
||||
nav2_params_file = PathJoinSubstitution(
|
||||
[FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml']
|
||||
)
|
||||
|
||||
# Paths
|
||||
gazebo_launch = PathJoinSubstitution(
|
||||
[pkg_turtlebot3_gazebo, 'launch', f'turtlebot3_{world}.launch.py'])
|
||||
nav2_launch = PathJoinSubstitution(
|
||||
[pkg_nav2_bringup, 'launch', 'navigation_launch.py'])
|
||||
rviz_launch = PathJoinSubstitution(
|
||||
[pkg_nav2_bringup, 'launch', 'rviz_launch.py'])
|
||||
rtabmap_launch = PathJoinSubstitution(
|
||||
[pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd.launch.py'])
|
||||
|
||||
# Includes
|
||||
gazebo = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([gazebo_launch]),
|
||||
launch_arguments=[
|
||||
('x_pose', LaunchConfiguration('x_pose')),
|
||||
('y_pose', LaunchConfiguration('y_pose'))
|
||||
]
|
||||
)
|
||||
nav2 = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([nav2_launch]),
|
||||
launch_arguments=[
|
||||
('use_sim_time', 'true'),
|
||||
('params_file', nav2_params_file)
|
||||
]
|
||||
)
|
||||
rviz = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([rviz_launch])
|
||||
)
|
||||
rtabmap = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([rtabmap_launch]),
|
||||
launch_arguments=[
|
||||
('localization', LaunchConfiguration('localization')),
|
||||
('use_sim_time', 'true')
|
||||
]
|
||||
)
|
||||
return [
|
||||
# Nodes to launch
|
||||
nav2,
|
||||
rviz,
|
||||
rtabmap,
|
||||
gazebo
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Launch in localization mode.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'world', default_value='house',
|
||||
choices=['world', 'house', 'dqn_stage1', 'dqn_stage2', 'dqn_stage3', 'dqn_stage4'],
|
||||
description='Turtlebot3 gazebo world.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'x_pose', default_value='-2.0',
|
||||
description='Initial position of the robot in the simulator.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'y_pose', default_value='0.5',
|
||||
description='Initial position of the robot in the simulator.'),
|
||||
|
||||
OpaqueFunction(function=launch_setup)
|
||||
])
|
||||
+117
@@ -0,0 +1,117 @@
|
||||
# Requirements:
|
||||
# Install Turtlebot3 packages
|
||||
# Modify turtlebot3_waffle SDF:
|
||||
# 1) Edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf
|
||||
# 2) Add
|
||||
# <joint name="camera_rgb_optical_joint" type="fixed">
|
||||
# <parent>camera_rgb_frame</parent>
|
||||
# <child>camera_rgb_optical_frame</child>
|
||||
# <pose>0 0 0 -1.57079632679 0 -1.57079632679</pose>
|
||||
# <axis>
|
||||
# <xyz>0 0 1</xyz>
|
||||
# </axis>
|
||||
# </joint>
|
||||
# 3) Rename <link name="camera_rgb_frame"> to <link name="camera_rgb_optical_frame">
|
||||
# 4) Add <link name="camera_rgb_frame"/>
|
||||
# 5) Change <sensor name="camera" type="camera"> to <sensor name="camera" type="depth">
|
||||
# 6) Change image width/height from 1920x1080 to 640x480
|
||||
# Example:
|
||||
# $ ros2 launch rtabmap_demos turtlebot3_sim_rgbd_fake_scan_demo.launch.py
|
||||
#
|
||||
# Teleop:
|
||||
# $ ros2 run turtlebot3_teleop teleop_keyboard
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
import os
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
if not 'TURTLEBOT3_MODEL' in os.environ:
|
||||
os.environ['TURTLEBOT3_MODEL'] = 'waffle'
|
||||
|
||||
# Directories
|
||||
pkg_turtlebot3_gazebo = get_package_share_directory(
|
||||
'turtlebot3_gazebo')
|
||||
pkg_nav2_bringup = get_package_share_directory(
|
||||
'nav2_bringup')
|
||||
pkg_rtabmap_demos = get_package_share_directory(
|
||||
'rtabmap_demos')
|
||||
|
||||
world = LaunchConfiguration('world').perform(context)
|
||||
|
||||
nav2_params_file = PathJoinSubstitution(
|
||||
[FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_nav2_params.yaml']
|
||||
)
|
||||
|
||||
# Paths
|
||||
gazebo_launch = PathJoinSubstitution(
|
||||
[pkg_turtlebot3_gazebo, 'launch', f'turtlebot3_{world}.launch.py'])
|
||||
nav2_launch = PathJoinSubstitution(
|
||||
[pkg_nav2_bringup, 'launch', 'navigation_launch.py'])
|
||||
rviz_launch = PathJoinSubstitution(
|
||||
[pkg_nav2_bringup, 'launch', 'rviz_launch.py'])
|
||||
rtabmap_launch = PathJoinSubstitution(
|
||||
[pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd_fake_scan.launch.py'])
|
||||
|
||||
# Includes
|
||||
gazebo = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([gazebo_launch]),
|
||||
launch_arguments=[
|
||||
('x_pose', LaunchConfiguration('x_pose')),
|
||||
('y_pose', LaunchConfiguration('y_pose'))
|
||||
]
|
||||
)
|
||||
nav2 = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([nav2_launch]),
|
||||
launch_arguments=[
|
||||
('use_sim_time', 'true'),
|
||||
('params_file', nav2_params_file)
|
||||
]
|
||||
)
|
||||
rviz = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([rviz_launch])
|
||||
)
|
||||
rtabmap = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([rtabmap_launch]),
|
||||
launch_arguments=[
|
||||
('localization', LaunchConfiguration('localization')),
|
||||
('use_sim_time', 'true')
|
||||
]
|
||||
)
|
||||
return [
|
||||
# Nodes to launch
|
||||
nav2,
|
||||
rviz,
|
||||
rtabmap,
|
||||
gazebo
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Launch in localization mode.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'world', default_value='house',
|
||||
choices=['world', 'house', 'dqn_stage1', 'dqn_stage2', 'dqn_stage3', 'dqn_stage4'],
|
||||
description='Turtlebot3 gazebo world.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'x_pose', default_value='-2.0',
|
||||
description='Initial position of the robot in the simulator.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'y_pose', default_value='0.5',
|
||||
description='Initial position of the robot in the simulator.'),
|
||||
|
||||
OpaqueFunction(function=launch_setup)
|
||||
])
|
||||
@@ -0,0 +1,119 @@
|
||||
# Requirements:
|
||||
# Install Turtlebot3 packages
|
||||
# Modify turtlebot3_waffle SDF:
|
||||
# 1) Edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf
|
||||
# 2) Add
|
||||
# <joint name="camera_rgb_optical_joint" type="fixed">
|
||||
# <parent>camera_rgb_frame</parent>
|
||||
# <child>camera_rgb_optical_frame</child>
|
||||
# <pose>0 0 0 -1.57079632679 0 -1.57079632679</pose>
|
||||
# <axis>
|
||||
# <xyz>0 0 1</xyz>
|
||||
# </axis>
|
||||
# </joint>
|
||||
# 3) Rename <link name="camera_rgb_frame"> to <link name="camera_rgb_optical_frame">
|
||||
# 4) Add <link name="camera_rgb_frame"/>
|
||||
# 5) Change <sensor name="camera" type="camera"> to <sensor name="camera" type="depth">
|
||||
# 6) Change image width/height from 1920x1080 to 640x480
|
||||
# 7) Note that we can increase min scan range from 0.12 to 0.2 to avoid having scans
|
||||
# hitting the robot itself
|
||||
# Example:
|
||||
# $ ros2 launch rtabmap_demos turtlebot3_sim_rgbd_scan_demo.launch.py
|
||||
#
|
||||
# Teleop:
|
||||
# $ ros2 run turtlebot3_teleop teleop_keyboard
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
import os
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
if not 'TURTLEBOT3_MODEL' in os.environ:
|
||||
os.environ['TURTLEBOT3_MODEL'] = 'waffle'
|
||||
|
||||
# Directories
|
||||
pkg_turtlebot3_gazebo = get_package_share_directory(
|
||||
'turtlebot3_gazebo')
|
||||
pkg_nav2_bringup = get_package_share_directory(
|
||||
'nav2_bringup')
|
||||
pkg_rtabmap_demos = get_package_share_directory(
|
||||
'rtabmap_demos')
|
||||
|
||||
world = LaunchConfiguration('world').perform(context)
|
||||
|
||||
nav2_params_file = PathJoinSubstitution(
|
||||
[FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_rgbd_scan_nav2_params.yaml']
|
||||
)
|
||||
|
||||
# Paths
|
||||
gazebo_launch = PathJoinSubstitution(
|
||||
[pkg_turtlebot3_gazebo, 'launch', f'turtlebot3_{world}.launch.py'])
|
||||
nav2_launch = PathJoinSubstitution(
|
||||
[pkg_nav2_bringup, 'launch', 'navigation_launch.py'])
|
||||
rviz_launch = PathJoinSubstitution(
|
||||
[pkg_nav2_bringup, 'launch', 'rviz_launch.py'])
|
||||
rtabmap_launch = PathJoinSubstitution(
|
||||
[pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_rgbd_scan.launch.py'])
|
||||
|
||||
# Includes
|
||||
gazebo = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([gazebo_launch]),
|
||||
launch_arguments=[
|
||||
('x_pose', LaunchConfiguration('x_pose')),
|
||||
('y_pose', LaunchConfiguration('y_pose'))
|
||||
]
|
||||
)
|
||||
nav2 = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([nav2_launch]),
|
||||
launch_arguments=[
|
||||
('use_sim_time', 'true'),
|
||||
('params_file', nav2_params_file)
|
||||
]
|
||||
)
|
||||
rviz = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([rviz_launch])
|
||||
)
|
||||
rtabmap = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([rtabmap_launch]),
|
||||
launch_arguments=[
|
||||
('localization', LaunchConfiguration('localization')),
|
||||
('use_sim_time', 'true')
|
||||
]
|
||||
)
|
||||
return [
|
||||
# Nodes to launch
|
||||
nav2,
|
||||
rviz,
|
||||
rtabmap,
|
||||
gazebo
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Launch in localization mode.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'world', default_value='house',
|
||||
choices=['world', 'house', 'dqn_stage1', 'dqn_stage2', 'dqn_stage3', 'dqn_stage4'],
|
||||
description='Turtlebot3 gazebo world.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'x_pose', default_value='-2.0',
|
||||
description='Initial position of the robot in the simulator.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'y_pose', default_value='0.5',
|
||||
description='Initial position of the robot in the simulator.'),
|
||||
|
||||
OpaqueFunction(function=launch_setup)
|
||||
])
|
||||
@@ -0,0 +1,161 @@
|
||||
# Requirements:
|
||||
# Install Turtlebot3 packages
|
||||
# Modify turtlebot3_waffle SDF:
|
||||
# 1) Edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot3_waffle/model.sdf
|
||||
# 2) We can increase min scan range from 0.12 to 0.2 to avoid having scans
|
||||
# hitting the robot itself
|
||||
#
|
||||
# Example:
|
||||
# $ ros2 launch rtabmap_demos turtlebot3_sim_scan_demo.launch.py
|
||||
#
|
||||
# Teleop:
|
||||
# $ ros2 run turtlebot3_teleop teleop_keyboard
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
|
||||
import os
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
if not 'TURTLEBOT3_MODEL' in os.environ:
|
||||
os.environ['TURTLEBOT3_MODEL'] = 'waffle'
|
||||
|
||||
# Directories
|
||||
pkg_nav2_bringup = get_package_share_directory(
|
||||
'nav2_bringup')
|
||||
pkg_rtabmap_demos = get_package_share_directory(
|
||||
'rtabmap_demos')
|
||||
|
||||
world_name = LaunchConfiguration('world').perform(context)
|
||||
|
||||
icp_odometry = LaunchConfiguration('icp_odometry').perform(context)
|
||||
icp_odometry = icp_odometry == 'True' or icp_odometry == 'true'
|
||||
if icp_odometry:
|
||||
# modified nav2 params to use icp_odom instead odom frame
|
||||
nav2_params_file = PathJoinSubstitution(
|
||||
[FindPackageShare('rtabmap_demos'), 'params', 'turtlebot3_scan_nav2_params.yaml']
|
||||
)
|
||||
else:
|
||||
# original nav2 params
|
||||
nav2_params_file = PathJoinSubstitution(
|
||||
[FindPackageShare('nav2_bringup'), 'params', 'nav2_params.yaml']
|
||||
)
|
||||
|
||||
# Paths
|
||||
nav2_launch = PathJoinSubstitution(
|
||||
[pkg_nav2_bringup, 'launch', 'navigation_launch.py'])
|
||||
rviz_launch = PathJoinSubstitution(
|
||||
[pkg_nav2_bringup, 'launch', 'rviz_launch.py'])
|
||||
rtabmap_launch = PathJoinSubstitution(
|
||||
[pkg_rtabmap_demos, 'launch', 'turtlebot3', 'turtlebot3_scan.launch.py'])
|
||||
|
||||
# To use ICP odometry, we should increase clock rate of gazebo, we copied content of
|
||||
# turtlebot3_gazebo/launch/turtlebot3_world.launch here
|
||||
launch_file_dir = os.path.join(get_package_share_directory('turtlebot3_gazebo'), 'launch')
|
||||
pkg_gazebo_ros = get_package_share_directory('gazebo_ros')
|
||||
|
||||
world = os.path.join(
|
||||
get_package_share_directory('turtlebot3_gazebo'),
|
||||
'worlds',
|
||||
f'turtlebot3_{world_name}.world'
|
||||
)
|
||||
|
||||
import tempfile
|
||||
with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as clock_override_file:
|
||||
clock_override_file.write("---\n"+
|
||||
"gazebo:\n"+
|
||||
" ros__parameters:\n"+
|
||||
" publish_rate: 100.0")
|
||||
|
||||
gzserver_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')
|
||||
),
|
||||
launch_arguments={
|
||||
'world': world,
|
||||
'params_file': clock_override_file.name}.items()
|
||||
)
|
||||
|
||||
gzclient_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')
|
||||
)
|
||||
)
|
||||
|
||||
robot_state_publisher_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(launch_file_dir, 'robot_state_publisher.launch.py')
|
||||
),
|
||||
launch_arguments={'use_sim_time': 'true'}.items()
|
||||
)
|
||||
|
||||
spawn_turtlebot_cmd = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(launch_file_dir, 'spawn_turtlebot3.launch.py')
|
||||
),
|
||||
launch_arguments={
|
||||
'x_pose': LaunchConfiguration('x_pose'),
|
||||
'y_pose': LaunchConfiguration('y_pose')
|
||||
}.items()
|
||||
)
|
||||
|
||||
nav2 = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([nav2_launch]),
|
||||
launch_arguments=[
|
||||
('use_sim_time', 'true'),
|
||||
('params_file', nav2_params_file)
|
||||
]
|
||||
)
|
||||
rviz = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([rviz_launch])
|
||||
)
|
||||
rtabmap = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([rtabmap_launch]),
|
||||
launch_arguments=[
|
||||
('localization', LaunchConfiguration('localization')),
|
||||
('use_sim_time', 'true')
|
||||
]
|
||||
)
|
||||
return [
|
||||
# Nodes to launch
|
||||
nav2,
|
||||
rviz,
|
||||
rtabmap,
|
||||
gzserver_cmd,
|
||||
gzclient_cmd,
|
||||
robot_state_publisher_cmd,
|
||||
spawn_turtlebot_cmd
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Launch in localization mode.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'world', default_value='world',
|
||||
choices=['world', 'house', 'dqn_stage1', 'dqn_stage2', 'dqn_stage3', 'dqn_stage4'],
|
||||
description='Turtlebot3 gazebo world.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'icp_odometry', default_value='false',
|
||||
description='Launch ICP odometry on top of wheel odometry.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'x_pose', default_value='-2.0',
|
||||
description='Initial position of the robot in the simulator.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'y_pose', default_value='0.5',
|
||||
description='Initial position of the robot in the simulator.'),
|
||||
|
||||
OpaqueFunction(function=launch_setup)
|
||||
])
|
||||
@@ -0,0 +1,79 @@
|
||||
#
|
||||
# Note: Make sure you have this fix for turtlebot4_description https://github.com/turtlebot/turtlebot4/pull/434,
|
||||
# otherwise, the lidar and camera point cloud won't be aligned correctly.
|
||||
#
|
||||
# Example:
|
||||
# 1) Launch simulator (turtlebot4, nav2 and rtabmap):
|
||||
# $ ros2 launch rtabmap_demos turtlebot4_sim_demo.launch.py
|
||||
#
|
||||
# 2) Click on "Play" button on bottom-left of gazebo.
|
||||
#
|
||||
# 3) Click on double points ".." button on top-right next to power button to undock.
|
||||
#
|
||||
# 4) Move the robot:
|
||||
# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
|
||||
# a) By teleoperating:
|
||||
# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard
|
||||
# c) By using autonomous exploration node (tested with https://github.com/robo-friends/m-explore-ros2):
|
||||
# $ ros2 launch explore_lite explore.launch.py
|
||||
#
|
||||
|
||||
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, PathJoinSubstitution
|
||||
|
||||
ARGUMENTS = [
|
||||
DeclareLaunchArgument('rviz', default_value='true',
|
||||
choices=['true', 'false'], description='Start rviz.'),
|
||||
DeclareLaunchArgument('rtabmap_viz', default_value='true',
|
||||
choices=['true', 'false'], description='Start rtabmap_viz.'),
|
||||
DeclareLaunchArgument('localization', default_value='false',
|
||||
choices=['true', 'false'], description='Start rtabmap in localization mode (a map should have been already created).'),
|
||||
DeclareLaunchArgument('nav2', default_value='true',
|
||||
choices=['true', 'false'], description='Start nav2.'),
|
||||
DeclareLaunchArgument('world', default_value='warehouse',
|
||||
description='Ignition World'),
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
# Directories
|
||||
pkg_turtlebot4_ignition_bringup = get_package_share_directory(
|
||||
'turtlebot4_ignition_bringup')
|
||||
pkg_rtabmap_demos = get_package_share_directory(
|
||||
'rtabmap_demos')
|
||||
|
||||
# Paths
|
||||
ignition_launch = PathJoinSubstitution(
|
||||
[pkg_turtlebot4_ignition_bringup, 'launch', 'turtlebot4_ignition.launch.py'])
|
||||
rtabmap_launch = PathJoinSubstitution(
|
||||
[pkg_rtabmap_demos, 'launch', 'turtlebot4', 'turtlebot4_slam.launch.py'])
|
||||
|
||||
ignition = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([ignition_launch]),
|
||||
launch_arguments=[
|
||||
('world', LaunchConfiguration('world')),
|
||||
('slam', 'false'),
|
||||
('localization', 'false'),
|
||||
('nav2', LaunchConfiguration('nav2')),
|
||||
('rviz', LaunchConfiguration('rviz'))
|
||||
]
|
||||
)
|
||||
|
||||
rtabmap = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([rtabmap_launch]),
|
||||
launch_arguments=[
|
||||
('rtabmap_viz', LaunchConfiguration('rtabmap_viz')),
|
||||
('localization', LaunchConfiguration('localization')),
|
||||
('use_sim_time', 'true')
|
||||
]
|
||||
)
|
||||
|
||||
# Create launch description and add actions
|
||||
ld = LaunchDescription(ARGUMENTS)
|
||||
ld.add_action(rtabmap) # put it first so that localization arg is not overwritten by the same used by ignition
|
||||
ld.add_action(ignition)
|
||||
return ld
|
||||
@@ -0,0 +1,119 @@
|
||||
#
|
||||
# Note: Make sure you have this fix for turtlebot4_description https://github.com/turtlebot/turtlebot4/pull/434,
|
||||
# otherwise, the lidar and camera point cloud won't be aligned correctly.
|
||||
#
|
||||
# Example with gazebo:
|
||||
# 1) Launch simulator (turtlebot4 and nav2):
|
||||
# $ ros2 launch turtlebot4_ignition_bringup turtlebot4_ignition.launch.py slam:=false nav2:=true rviz:=true
|
||||
#
|
||||
# 2) Launch SLAM:
|
||||
# $ ros2 launch rtabmap_demos turtlebot4_slam.launch.py use_sim_time:=true
|
||||
# OR
|
||||
# $ ros2 launch rtabmap_launch rtabmap.launch.py rtabmap_viz:=true subscribe_scan:=true rgbd_sync:=true depth_topic:=/oakd/rgb/preview/depth odom_sensor_sync:=true camera_info_topic:=/oakd/rgb/preview/camera_info rgb_topic:=/oakd/rgb/preview/image_raw visual_odometry:=false approx_sync:=true approx_rgbd_sync:=false odom_guess_frame_id:=odom icp_odometry:=true odom_topic:="icp_odom" map_topic:="/map" use_sim_time:=true odom_log_level:=warn rtabmap_args:="--delete_db_on_start --Reg/Strategy 1 --Reg/Force3DoF true --Mem/NotLinkedNodesKept false" use_action_for_goal:=true
|
||||
#
|
||||
# 3) Click on "Play" button on bottom-left of gazebo.
|
||||
#
|
||||
# 4) Click on double points ".." button on top-right next to power button to undock.
|
||||
#
|
||||
# 5) Move the robot:
|
||||
# b) By sending goals with RVIZ's "Nav2 Goal" button in action bar.
|
||||
# a) By teleoperating:
|
||||
# $ ros2 run teleop_twist_keyboard teleop_twist_keyboard
|
||||
# c) By using autonomous exploration node (tested with https://github.com/robo-friends/m-explore-ros2):
|
||||
# $ ros2 launch explore_lite explore.launch.py
|
||||
#
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
localization = LaunchConfiguration('localization')
|
||||
rtabmap_viz = LaunchConfiguration('rtabmap_viz')
|
||||
|
||||
icp_parameters={
|
||||
'odom_frame_id':'icp_odom',
|
||||
'guess_frame_id':'odom'
|
||||
}
|
||||
|
||||
rtabmap_parameters={
|
||||
'subscribe_rgbd':True,
|
||||
'subscribe_scan':True,
|
||||
'use_action_for_goal':True,
|
||||
'odom_sensor_sync': True,
|
||||
# RTAB-Map's parameters should be strings:
|
||||
'Mem/NotLinkedNodesKept':'false'
|
||||
}
|
||||
|
||||
# Shared parameters between different nodes
|
||||
shared_parameters={
|
||||
'frame_id':'base_link',
|
||||
'use_sim_time':use_sim_time,
|
||||
# RTAB-Map's parameters should be strings:
|
||||
'Reg/Strategy':'1',
|
||||
'Reg/Force3DoF':'true',
|
||||
'Mem/NotLinkedNodesKept':'false',
|
||||
'Icp/PointToPlaneMinComplexity':'0.04' # to be more robust to long corridors with low geometry
|
||||
}
|
||||
|
||||
remappings=[
|
||||
('odom', 'icp_odom'),
|
||||
('rgb/image', '/oakd/rgb/preview/image_raw'),
|
||||
('rgb/camera_info', '/oakd/rgb/preview/camera_info'),
|
||||
('depth/image', '/oakd/rgb/preview/depth')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='false', choices=['true', 'false'],
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false', choices=['true', 'false'],
|
||||
description='Launch rtabmap in localization mode (a map should have been already created).'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'rtabmap_viz', default_value='true', choices=['true', 'false'],
|
||||
description='Launch rtabmap_viz for visualization.'),
|
||||
|
||||
# Nodes to launch
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}],
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
package='rtabmap_odom', executable='icp_odometry', output='screen',
|
||||
parameters=[icp_parameters, shared_parameters],
|
||||
remappings=remappings,
|
||||
arguments=["--ros-args", "--log-level", 'icp_odometry:=warn']),
|
||||
|
||||
# SLAM Mode:
|
||||
Node(
|
||||
condition=UnlessCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[rtabmap_parameters, shared_parameters],
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
# Localization mode:
|
||||
Node(
|
||||
condition=IfCondition(localization),
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[rtabmap_parameters, shared_parameters,
|
||||
{'Mem/IncrementalMemory':'False',
|
||||
'Mem/InitWMWithAllNodes':'True'}],
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
condition=IfCondition(rtabmap_viz),
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=[rtabmap_parameters, shared_parameters],
|
||||
remappings=remappings),
|
||||
])
|
||||
@@ -0,0 +1,26 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>rtabmap_demos</name>
|
||||
<version>0.22.0</version>
|
||||
<description>RTAB-Map's demo launch files.</description>
|
||||
<maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
|
||||
<author>Mathieu Labbe</author>
|
||||
<license>BSD</license>
|
||||
<url type="bugtracker">https://github.com/introlab/rtabmap_ros/issues</url>
|
||||
<url type="repository">https://github.com/introlab/rtabmap_ros</url>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<exec_depend>rtabmap_odom</exec_depend>
|
||||
<exec_depend>rtabmap_slam</exec_depend>
|
||||
<exec_depend>rtabmap_util</exec_depend>
|
||||
<exec_depend>rtabmap_rviz_plugins</exec_depend>
|
||||
<exec_depend>rtabmap_viz</exec_depend>
|
||||
<exec_depend>nav2_bringup</exec_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
|
||||
</package>
|
||||
@@ -0,0 +1,288 @@
|
||||
# rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap.
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /odom
|
||||
bt_loop_duration: 10
|
||||
default_server_timeout: 20
|
||||
wait_for_service_timeout: 1000
|
||||
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
|
||||
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
|
||||
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
|
||||
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_smooth_path_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_assisted_teleop_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_drive_on_heading_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_globally_updated_goal_condition_bt_node
|
||||
- nav2_is_path_valid_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_truncate_path_local_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_path_expiring_timer_condition
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_goal_updated_controller_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_planner_selector_bt_node
|
||||
- nav2_controller_selector_bt_node
|
||||
- nav2_goal_checker_selector_bt_node
|
||||
- nav2_controller_cancel_bt_node
|
||||
- nav2_path_longer_on_approach_bt_node
|
||||
- nav2_wait_cancel_bt_node
|
||||
- nav2_spin_cancel_bt_node
|
||||
- nav2_back_up_cancel_bt_node
|
||||
- nav2_assisted_teleop_cancel_bt_node
|
||||
- nav2_drive_on_heading_cancel_bt_node
|
||||
- nav2_is_battery_charging_condition_bt_node
|
||||
|
||||
bt_navigator_navigate_through_poses_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
bt_navigator_navigate_to_pose_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
controller_frequency: 20.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.5
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 0.3
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
# Progress checker parameters
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
# Goal checker parameters
|
||||
#precise_goal_checker:
|
||||
# plugin: "nav2_controller::SimpleGoalChecker"
|
||||
# xy_goal_tolerance: 0.25
|
||||
# yaw_goal_tolerance: 0.25
|
||||
# stateful: True
|
||||
general_goal_checker:
|
||||
stateful: True
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
# DWB parameters
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
debug_trajectory_details: True
|
||||
min_vel_x: 0.0
|
||||
min_vel_y: 0.0
|
||||
max_vel_x: 0.26
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 1.0
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 0.26
|
||||
min_speed_theta: 0.0
|
||||
# Add high threshold velocity for turtlebot 3 issue.
|
||||
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
|
||||
acc_lim_x: 2.5
|
||||
acc_lim_y: 0.0
|
||||
acc_lim_theta: 3.2
|
||||
decel_lim_x: -2.5
|
||||
decel_lim_y: 0.0
|
||||
decel_lim_theta: -3.2
|
||||
vx_samples: 20
|
||||
vy_samples: 5
|
||||
vtheta_samples: 20
|
||||
sim_time: 1.7
|
||||
linear_granularity: 0.05
|
||||
angular_granularity: 0.025
|
||||
transform_tolerance: 0.2
|
||||
xy_goal_tolerance: 0.25
|
||||
trans_stopped_velocity: 0.25
|
||||
short_circuit_trajectory_evaluation: True
|
||||
stateful: True
|
||||
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
|
||||
BaseObstacle.scale: 0.02
|
||||
PathAlign.scale: 32.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 24.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
RotateToGoal.scale: 32.0
|
||||
RotateToGoal.slowing_factor: 5.0
|
||||
RotateToGoal.lookahead_time: -1.0
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 5.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
robot_radius: 0.22
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: ground obstacles
|
||||
ground:
|
||||
topic: /camera/ground
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: False
|
||||
data_type: "PointCloud2"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
obstacles:
|
||||
topic: /camera/obstacles
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "PointCloud2"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
always_send_full_costmap: True
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
robot_radius: 0.22
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "inflation_layer"]
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
always_send_full_costmap: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 20.0
|
||||
use_sim_time: True
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoother_plugins: ["simple_smoother"]
|
||||
simple_smoother:
|
||||
plugin: "nav2_smoother::SimpleSmoother"
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: True
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors/Spin"
|
||||
backup:
|
||||
plugin: "nav2_behaviors/BackUp"
|
||||
drive_on_heading:
|
||||
plugin: "nav2_behaviors/DriveOnHeading"
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
assisted_teleop:
|
||||
plugin: "nav2_behaviors/AssistedTeleop"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.1
|
||||
use_sim_time: true
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
loop_rate: 20
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
|
||||
velocity_smoother:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoothing_frequency: 20.0
|
||||
scale_velocities: False
|
||||
feedback: "OPEN_LOOP"
|
||||
max_velocity: [0.26, 0.0, 1.0]
|
||||
min_velocity: [-0.26, 0.0, -1.0]
|
||||
max_accel: [2.5, 0.0, 3.2]
|
||||
max_decel: [-2.5, 0.0, -3.2]
|
||||
odom_topic: "odom"
|
||||
odom_duration: 0.1
|
||||
deadband_velocity: [0.0, 0.0, 0.0]
|
||||
velocity_timeout: 1.0
|
||||
@@ -0,0 +1,295 @@
|
||||
# Isaac example: We increased max velocities.
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /chassis/odom
|
||||
bt_loop_duration: 10
|
||||
default_server_timeout: 20
|
||||
wait_for_service_timeout: 1000
|
||||
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
|
||||
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
|
||||
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
|
||||
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_smooth_path_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_assisted_teleop_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_drive_on_heading_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_globally_updated_goal_condition_bt_node
|
||||
- nav2_is_path_valid_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_truncate_path_local_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_path_expiring_timer_condition
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_goal_updated_controller_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_planner_selector_bt_node
|
||||
- nav2_controller_selector_bt_node
|
||||
- nav2_goal_checker_selector_bt_node
|
||||
- nav2_controller_cancel_bt_node
|
||||
- nav2_path_longer_on_approach_bt_node
|
||||
- nav2_wait_cancel_bt_node
|
||||
- nav2_spin_cancel_bt_node
|
||||
- nav2_back_up_cancel_bt_node
|
||||
- nav2_assisted_teleop_cancel_bt_node
|
||||
- nav2_drive_on_heading_cancel_bt_node
|
||||
- nav2_is_battery_charging_condition_bt_node
|
||||
|
||||
bt_navigator_navigate_through_poses_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
bt_navigator_navigate_to_pose_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
controller_frequency: 20.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.5
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 0.3
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
# Progress checker parameters
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
# Goal checker parameters
|
||||
#precise_goal_checker:
|
||||
# plugin: "nav2_controller::SimpleGoalChecker"
|
||||
# xy_goal_tolerance: 0.25
|
||||
# yaw_goal_tolerance: 0.25
|
||||
# stateful: True
|
||||
general_goal_checker:
|
||||
stateful: True
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
# DWB parameters
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
debug_trajectory_details: True
|
||||
min_vel_x: 0.0
|
||||
min_vel_y: 0.0
|
||||
max_vel_x: 2.0
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 1.0
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 2.0
|
||||
min_speed_theta: 0.0
|
||||
# Add high threshold velocity for turtlebot 3 issue.
|
||||
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
|
||||
acc_lim_x: 2.5
|
||||
acc_lim_y: 0.0
|
||||
acc_lim_theta: 3.2
|
||||
decel_lim_x: -2.5
|
||||
decel_lim_y: 0.0
|
||||
decel_lim_theta: -3.2
|
||||
vx_samples: 20
|
||||
vy_samples: 5
|
||||
vtheta_samples: 20
|
||||
sim_time: 1.7
|
||||
linear_granularity: 0.05
|
||||
angular_granularity: 0.025
|
||||
transform_tolerance: 0.2
|
||||
xy_goal_tolerance: 0.25
|
||||
trans_stopped_velocity: 0.25
|
||||
short_circuit_trajectory_evaluation: True
|
||||
stateful: True
|
||||
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
|
||||
BaseObstacle.scale: 0.02
|
||||
PathAlign.scale: 32.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 24.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
RotateToGoal.scale: 32.0
|
||||
RotateToGoal.slowing_factor: 5.0
|
||||
RotateToGoal.lookahead_time: -1.0
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
robot_radius: 0.22
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
always_send_full_costmap: True
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
robot_radius: 0.22
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
always_send_full_costmap: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 20.0
|
||||
use_sim_time: True
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoother_plugins: ["simple_smoother"]
|
||||
simple_smoother:
|
||||
plugin: "nav2_smoother::SimpleSmoother"
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: True
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors/Spin"
|
||||
backup:
|
||||
plugin: "nav2_behaviors/BackUp"
|
||||
drive_on_heading:
|
||||
plugin: "nav2_behaviors/DriveOnHeading"
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
assisted_teleop:
|
||||
plugin: "nav2_behaviors/AssistedTeleop"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.1
|
||||
use_sim_time: true
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
loop_rate: 20
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
|
||||
velocity_smoother:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoothing_frequency: 20.0
|
||||
scale_velocities: False
|
||||
feedback: "OPEN_LOOP"
|
||||
max_velocity: [2.0, 0.0, 2.0]
|
||||
min_velocity: [-2.0, 0.0, -2.0]
|
||||
max_accel: [2.5, 0.0, 3.2]
|
||||
max_decel: [-2.5, 0.0, -3.2]
|
||||
odom_topic: "/chassis/odom"
|
||||
odom_duration: 0.1
|
||||
deadband_velocity: [0.0, 0.0, 0.0]
|
||||
velocity_timeout: 1.0
|
||||
@@ -0,0 +1,295 @@
|
||||
# Isaac example: we changed the main odom_frame_id from "odom" to "vo" frame. We increased max velocities.
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /chassis/odom
|
||||
bt_loop_duration: 10
|
||||
default_server_timeout: 20
|
||||
wait_for_service_timeout: 1000
|
||||
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
|
||||
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
|
||||
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
|
||||
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_smooth_path_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_assisted_teleop_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_drive_on_heading_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_globally_updated_goal_condition_bt_node
|
||||
- nav2_is_path_valid_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_truncate_path_local_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_path_expiring_timer_condition
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_goal_updated_controller_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_planner_selector_bt_node
|
||||
- nav2_controller_selector_bt_node
|
||||
- nav2_goal_checker_selector_bt_node
|
||||
- nav2_controller_cancel_bt_node
|
||||
- nav2_path_longer_on_approach_bt_node
|
||||
- nav2_wait_cancel_bt_node
|
||||
- nav2_spin_cancel_bt_node
|
||||
- nav2_back_up_cancel_bt_node
|
||||
- nav2_assisted_teleop_cancel_bt_node
|
||||
- nav2_drive_on_heading_cancel_bt_node
|
||||
- nav2_is_battery_charging_condition_bt_node
|
||||
|
||||
bt_navigator_navigate_through_poses_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
bt_navigator_navigate_to_pose_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
controller_frequency: 20.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.5
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 0.3
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
# Progress checker parameters
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
# Goal checker parameters
|
||||
#precise_goal_checker:
|
||||
# plugin: "nav2_controller::SimpleGoalChecker"
|
||||
# xy_goal_tolerance: 0.25
|
||||
# yaw_goal_tolerance: 0.25
|
||||
# stateful: True
|
||||
general_goal_checker:
|
||||
stateful: True
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
# DWB parameters
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
debug_trajectory_details: True
|
||||
min_vel_x: 0.0
|
||||
min_vel_y: 0.0
|
||||
max_vel_x: 2.0
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 1.0
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 2.0
|
||||
min_speed_theta: 0.0
|
||||
# Add high threshold velocity for turtlebot 3 issue.
|
||||
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
|
||||
acc_lim_x: 2.5
|
||||
acc_lim_y: 0.0
|
||||
acc_lim_theta: 3.2
|
||||
decel_lim_x: -2.5
|
||||
decel_lim_y: 0.0
|
||||
decel_lim_theta: -3.2
|
||||
vx_samples: 20
|
||||
vy_samples: 5
|
||||
vtheta_samples: 20
|
||||
sim_time: 1.7
|
||||
linear_granularity: 0.05
|
||||
angular_granularity: 0.025
|
||||
transform_tolerance: 0.2
|
||||
xy_goal_tolerance: 0.25
|
||||
trans_stopped_velocity: 0.25
|
||||
short_circuit_trajectory_evaluation: True
|
||||
stateful: True
|
||||
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
|
||||
BaseObstacle.scale: 0.02
|
||||
PathAlign.scale: 32.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 24.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
RotateToGoal.scale: 32.0
|
||||
RotateToGoal.slowing_factor: 5.0
|
||||
RotateToGoal.lookahead_time: -1.0
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: vo
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
robot_radius: 0.22
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
always_send_full_costmap: True
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
robot_radius: 0.22
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
always_send_full_costmap: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 20.0
|
||||
use_sim_time: True
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoother_plugins: ["simple_smoother"]
|
||||
simple_smoother:
|
||||
plugin: "nav2_smoother::SimpleSmoother"
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: True
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors/Spin"
|
||||
backup:
|
||||
plugin: "nav2_behaviors/BackUp"
|
||||
drive_on_heading:
|
||||
plugin: "nav2_behaviors/DriveOnHeading"
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
assisted_teleop:
|
||||
plugin: "nav2_behaviors/AssistedTeleop"
|
||||
global_frame: vo
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.1
|
||||
use_sim_time: true
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
loop_rate: 20
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
|
||||
velocity_smoother:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoothing_frequency: 20.0
|
||||
scale_velocities: False
|
||||
feedback: "OPEN_LOOP"
|
||||
max_velocity: [2.0, 0.0, 2.0]
|
||||
min_velocity: [-2.0, 0.0, -2.0]
|
||||
max_accel: [2.5, 0.0, 3.2]
|
||||
max_decel: [-2.5, 0.0, -3.2]
|
||||
odom_topic: "/chassis/odom"
|
||||
odom_duration: 0.1
|
||||
deadband_velocity: [0.0, 0.0, 0.0]
|
||||
velocity_timeout: 1.0
|
||||
@@ -0,0 +1,287 @@
|
||||
# rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap and removed scan source.
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /odom
|
||||
bt_loop_duration: 10
|
||||
default_server_timeout: 20
|
||||
wait_for_service_timeout: 1000
|
||||
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
|
||||
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
|
||||
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
|
||||
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_smooth_path_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_assisted_teleop_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_drive_on_heading_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_globally_updated_goal_condition_bt_node
|
||||
- nav2_is_path_valid_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_truncate_path_local_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_path_expiring_timer_condition
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_goal_updated_controller_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_planner_selector_bt_node
|
||||
- nav2_controller_selector_bt_node
|
||||
- nav2_goal_checker_selector_bt_node
|
||||
- nav2_controller_cancel_bt_node
|
||||
- nav2_path_longer_on_approach_bt_node
|
||||
- nav2_wait_cancel_bt_node
|
||||
- nav2_spin_cancel_bt_node
|
||||
- nav2_back_up_cancel_bt_node
|
||||
- nav2_assisted_teleop_cancel_bt_node
|
||||
- nav2_drive_on_heading_cancel_bt_node
|
||||
- nav2_is_battery_charging_condition_bt_node
|
||||
|
||||
bt_navigator_navigate_through_poses_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
bt_navigator_navigate_to_pose_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
controller_frequency: 20.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.5
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 0.3
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
# Progress checker parameters
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
# Goal checker parameters
|
||||
#precise_goal_checker:
|
||||
# plugin: "nav2_controller::SimpleGoalChecker"
|
||||
# xy_goal_tolerance: 0.25
|
||||
# yaw_goal_tolerance: 0.25
|
||||
# stateful: True
|
||||
general_goal_checker:
|
||||
stateful: True
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
# DWB parameters
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
debug_trajectory_details: True
|
||||
min_vel_x: 0.0
|
||||
min_vel_y: 0.0
|
||||
max_vel_x: 0.26
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 1.0
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 0.26
|
||||
min_speed_theta: 0.0
|
||||
# Add high threshold velocity for turtlebot 3 issue.
|
||||
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
|
||||
acc_lim_x: 2.5
|
||||
acc_lim_y: 0.0
|
||||
acc_lim_theta: 3.2
|
||||
decel_lim_x: -2.5
|
||||
decel_lim_y: 0.0
|
||||
decel_lim_theta: -3.2
|
||||
vx_samples: 20
|
||||
vy_samples: 5
|
||||
vtheta_samples: 20
|
||||
sim_time: 1.7
|
||||
linear_granularity: 0.05
|
||||
angular_granularity: 0.025
|
||||
transform_tolerance: 0.2
|
||||
xy_goal_tolerance: 0.25
|
||||
trans_stopped_velocity: 0.25
|
||||
short_circuit_trajectory_evaluation: True
|
||||
stateful: True
|
||||
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
|
||||
BaseObstacle.scale: 0.02
|
||||
PathAlign.scale: 32.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 24.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
RotateToGoal.scale: 32.0
|
||||
RotateToGoal.slowing_factor: 5.0
|
||||
RotateToGoal.lookahead_time: -1.0
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
robot_radius: 0.22
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: ground obstacles
|
||||
ground:
|
||||
topic: /camera/ground
|
||||
max_obstacle_height: 0.4
|
||||
clearing: True
|
||||
marking: False
|
||||
data_type: "PointCloud2"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
obstacles:
|
||||
topic: /camera/obstacles
|
||||
max_obstacle_height: 0.4
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "PointCloud2"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
always_send_full_costmap: True
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
robot_radius: 0.22
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "inflation_layer"]
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
always_send_full_costmap: True
|
||||
|
||||
map_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
# Overridden in launch by the "map" launch configuration or provided default value.
|
||||
# To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below.
|
||||
yaml_filename: ""
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoother_plugins: ["simple_smoother"]
|
||||
simple_smoother:
|
||||
plugin: "nav2_smoother::SimpleSmoother"
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: True
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors/Spin"
|
||||
backup:
|
||||
plugin: "nav2_behaviors/BackUp"
|
||||
drive_on_heading:
|
||||
plugin: "nav2_behaviors/DriveOnHeading"
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
assisted_teleop:
|
||||
plugin: "nav2_behaviors/AssistedTeleop"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.1
|
||||
use_sim_time: true
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
loop_rate: 20
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
|
||||
velocity_smoother:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoothing_frequency: 20.0
|
||||
scale_velocities: False
|
||||
feedback: "OPEN_LOOP"
|
||||
max_velocity: [0.26, 0.0, 1.0]
|
||||
min_velocity: [-0.26, 0.0, -1.0]
|
||||
max_accel: [2.5, 0.0, 3.2]
|
||||
max_decel: [-2.5, 0.0, -3.2]
|
||||
odom_topic: "odom"
|
||||
odom_duration: 0.1
|
||||
deadband_velocity: [0.0, 0.0, 0.0]
|
||||
velocity_timeout: 1.0
|
||||
@@ -0,0 +1,301 @@
|
||||
# rtabmap_demos: We add segmented ground and obstacles to voxel_layer of the local costmap.
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /odom
|
||||
bt_loop_duration: 10
|
||||
default_server_timeout: 20
|
||||
wait_for_service_timeout: 1000
|
||||
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
|
||||
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
|
||||
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
|
||||
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_smooth_path_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_assisted_teleop_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_drive_on_heading_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_globally_updated_goal_condition_bt_node
|
||||
- nav2_is_path_valid_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_truncate_path_local_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_path_expiring_timer_condition
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_goal_updated_controller_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_planner_selector_bt_node
|
||||
- nav2_controller_selector_bt_node
|
||||
- nav2_goal_checker_selector_bt_node
|
||||
- nav2_controller_cancel_bt_node
|
||||
- nav2_path_longer_on_approach_bt_node
|
||||
- nav2_wait_cancel_bt_node
|
||||
- nav2_spin_cancel_bt_node
|
||||
- nav2_back_up_cancel_bt_node
|
||||
- nav2_assisted_teleop_cancel_bt_node
|
||||
- nav2_drive_on_heading_cancel_bt_node
|
||||
- nav2_is_battery_charging_condition_bt_node
|
||||
|
||||
bt_navigator_navigate_through_poses_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
bt_navigator_navigate_to_pose_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
controller_frequency: 20.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.5
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 0.3
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
# Progress checker parameters
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
# Goal checker parameters
|
||||
#precise_goal_checker:
|
||||
# plugin: "nav2_controller::SimpleGoalChecker"
|
||||
# xy_goal_tolerance: 0.25
|
||||
# yaw_goal_tolerance: 0.25
|
||||
# stateful: True
|
||||
general_goal_checker:
|
||||
stateful: True
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
# DWB parameters
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
debug_trajectory_details: True
|
||||
min_vel_x: 0.0
|
||||
min_vel_y: 0.0
|
||||
max_vel_x: 0.26
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 1.0
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 0.26
|
||||
min_speed_theta: 0.0
|
||||
# Add high threshold velocity for turtlebot 3 issue.
|
||||
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
|
||||
acc_lim_x: 2.5
|
||||
acc_lim_y: 0.0
|
||||
acc_lim_theta: 3.2
|
||||
decel_lim_x: -2.5
|
||||
decel_lim_y: 0.0
|
||||
decel_lim_theta: -3.2
|
||||
vx_samples: 20
|
||||
vy_samples: 5
|
||||
vtheta_samples: 20
|
||||
sim_time: 1.7
|
||||
linear_granularity: 0.05
|
||||
angular_granularity: 0.025
|
||||
transform_tolerance: 0.2
|
||||
xy_goal_tolerance: 0.25
|
||||
trans_stopped_velocity: 0.25
|
||||
short_circuit_trajectory_evaluation: True
|
||||
stateful: True
|
||||
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
|
||||
BaseObstacle.scale: 0.02
|
||||
PathAlign.scale: 32.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 24.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
RotateToGoal.scale: 32.0
|
||||
RotateToGoal.slowing_factor: 5.0
|
||||
RotateToGoal.lookahead_time: -1.0
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
robot_radius: 0.22
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan ground obstacles
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
ground:
|
||||
topic: /camera/ground
|
||||
max_obstacle_height: 0.4
|
||||
clearing: True
|
||||
marking: False
|
||||
data_type: "PointCloud2"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
obstacles:
|
||||
topic: /camera/obstacles
|
||||
max_obstacle_height: 0.4
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "PointCloud2"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
always_send_full_costmap: True
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
robot_radius: 0.22
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "inflation_layer"]
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
always_send_full_costmap: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 20.0
|
||||
use_sim_time: True
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoother_plugins: ["simple_smoother"]
|
||||
simple_smoother:
|
||||
plugin: "nav2_smoother::SimpleSmoother"
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: True
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors/Spin"
|
||||
backup:
|
||||
plugin: "nav2_behaviors/BackUp"
|
||||
drive_on_heading:
|
||||
plugin: "nav2_behaviors/DriveOnHeading"
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
assisted_teleop:
|
||||
plugin: "nav2_behaviors/AssistedTeleop"
|
||||
global_frame: odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.1
|
||||
use_sim_time: true
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
loop_rate: 20
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
|
||||
velocity_smoother:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoothing_frequency: 20.0
|
||||
scale_velocities: False
|
||||
feedback: "OPEN_LOOP"
|
||||
max_velocity: [0.26, 0.0, 1.0]
|
||||
min_velocity: [-0.26, 0.0, -1.0]
|
||||
max_accel: [2.5, 0.0, 3.2]
|
||||
max_decel: [-2.5, 0.0, -3.2]
|
||||
odom_topic: "odom"
|
||||
odom_duration: 0.1
|
||||
deadband_velocity: [0.0, 0.0, 0.0]
|
||||
velocity_timeout: 1.0
|
||||
@@ -0,0 +1,295 @@
|
||||
# Modified to use icp_odom frame
|
||||
bt_navigator:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
odom_topic: /odom
|
||||
bt_loop_duration: 10
|
||||
default_server_timeout: 20
|
||||
wait_for_service_timeout: 1000
|
||||
# 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
|
||||
# nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
|
||||
# nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
|
||||
# They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.
|
||||
plugin_lib_names:
|
||||
- nav2_compute_path_to_pose_action_bt_node
|
||||
- nav2_compute_path_through_poses_action_bt_node
|
||||
- nav2_smooth_path_action_bt_node
|
||||
- nav2_follow_path_action_bt_node
|
||||
- nav2_spin_action_bt_node
|
||||
- nav2_wait_action_bt_node
|
||||
- nav2_assisted_teleop_action_bt_node
|
||||
- nav2_back_up_action_bt_node
|
||||
- nav2_drive_on_heading_bt_node
|
||||
- nav2_clear_costmap_service_bt_node
|
||||
- nav2_is_stuck_condition_bt_node
|
||||
- nav2_goal_reached_condition_bt_node
|
||||
- nav2_goal_updated_condition_bt_node
|
||||
- nav2_globally_updated_goal_condition_bt_node
|
||||
- nav2_is_path_valid_condition_bt_node
|
||||
- nav2_initial_pose_received_condition_bt_node
|
||||
- nav2_reinitialize_global_localization_service_bt_node
|
||||
- nav2_rate_controller_bt_node
|
||||
- nav2_distance_controller_bt_node
|
||||
- nav2_speed_controller_bt_node
|
||||
- nav2_truncate_path_action_bt_node
|
||||
- nav2_truncate_path_local_action_bt_node
|
||||
- nav2_goal_updater_node_bt_node
|
||||
- nav2_recovery_node_bt_node
|
||||
- nav2_pipeline_sequence_bt_node
|
||||
- nav2_round_robin_node_bt_node
|
||||
- nav2_transform_available_condition_bt_node
|
||||
- nav2_time_expired_condition_bt_node
|
||||
- nav2_path_expiring_timer_condition
|
||||
- nav2_distance_traveled_condition_bt_node
|
||||
- nav2_single_trigger_bt_node
|
||||
- nav2_goal_updated_controller_bt_node
|
||||
- nav2_is_battery_low_condition_bt_node
|
||||
- nav2_navigate_through_poses_action_bt_node
|
||||
- nav2_navigate_to_pose_action_bt_node
|
||||
- nav2_remove_passed_goals_action_bt_node
|
||||
- nav2_planner_selector_bt_node
|
||||
- nav2_controller_selector_bt_node
|
||||
- nav2_goal_checker_selector_bt_node
|
||||
- nav2_controller_cancel_bt_node
|
||||
- nav2_path_longer_on_approach_bt_node
|
||||
- nav2_wait_cancel_bt_node
|
||||
- nav2_spin_cancel_bt_node
|
||||
- nav2_back_up_cancel_bt_node
|
||||
- nav2_assisted_teleop_cancel_bt_node
|
||||
- nav2_drive_on_heading_cancel_bt_node
|
||||
- nav2_is_battery_charging_condition_bt_node
|
||||
|
||||
bt_navigator_navigate_through_poses_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
bt_navigator_navigate_to_pose_rclcpp_node:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
controller_frequency: 20.0
|
||||
min_x_velocity_threshold: 0.001
|
||||
min_y_velocity_threshold: 0.5
|
||||
min_theta_velocity_threshold: 0.001
|
||||
failure_tolerance: 0.3
|
||||
progress_checker_plugin: "progress_checker"
|
||||
goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
|
||||
controller_plugins: ["FollowPath"]
|
||||
|
||||
# Progress checker parameters
|
||||
progress_checker:
|
||||
plugin: "nav2_controller::SimpleProgressChecker"
|
||||
required_movement_radius: 0.5
|
||||
movement_time_allowance: 10.0
|
||||
# Goal checker parameters
|
||||
#precise_goal_checker:
|
||||
# plugin: "nav2_controller::SimpleGoalChecker"
|
||||
# xy_goal_tolerance: 0.25
|
||||
# yaw_goal_tolerance: 0.25
|
||||
# stateful: True
|
||||
general_goal_checker:
|
||||
stateful: True
|
||||
plugin: "nav2_controller::SimpleGoalChecker"
|
||||
xy_goal_tolerance: 0.25
|
||||
yaw_goal_tolerance: 0.25
|
||||
# DWB parameters
|
||||
FollowPath:
|
||||
plugin: "dwb_core::DWBLocalPlanner"
|
||||
debug_trajectory_details: True
|
||||
min_vel_x: 0.0
|
||||
min_vel_y: 0.0
|
||||
max_vel_x: 0.26
|
||||
max_vel_y: 0.0
|
||||
max_vel_theta: 1.0
|
||||
min_speed_xy: 0.0
|
||||
max_speed_xy: 0.26
|
||||
min_speed_theta: 0.0
|
||||
# Add high threshold velocity for turtlebot 3 issue.
|
||||
# https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
|
||||
acc_lim_x: 2.5
|
||||
acc_lim_y: 0.0
|
||||
acc_lim_theta: 3.2
|
||||
decel_lim_x: -2.5
|
||||
decel_lim_y: 0.0
|
||||
decel_lim_theta: -3.2
|
||||
vx_samples: 20
|
||||
vy_samples: 5
|
||||
vtheta_samples: 20
|
||||
sim_time: 1.7
|
||||
linear_granularity: 0.05
|
||||
angular_granularity: 0.025
|
||||
transform_tolerance: 0.2
|
||||
xy_goal_tolerance: 0.25
|
||||
trans_stopped_velocity: 0.25
|
||||
short_circuit_trajectory_evaluation: True
|
||||
stateful: True
|
||||
critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
|
||||
BaseObstacle.scale: 0.02
|
||||
PathAlign.scale: 32.0
|
||||
PathAlign.forward_point_distance: 0.1
|
||||
GoalAlign.scale: 24.0
|
||||
GoalAlign.forward_point_distance: 0.1
|
||||
PathDist.scale: 32.0
|
||||
GoalDist.scale: 24.0
|
||||
RotateToGoal.scale: 32.0
|
||||
RotateToGoal.slowing_factor: 5.0
|
||||
RotateToGoal.lookahead_time: -1.0
|
||||
|
||||
local_costmap:
|
||||
local_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 5.0
|
||||
publish_frequency: 2.0
|
||||
global_frame: icp_odom
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
rolling_window: true
|
||||
width: 3
|
||||
height: 3
|
||||
resolution: 0.05
|
||||
robot_radius: 0.22
|
||||
plugins: ["voxel_layer", "inflation_layer"]
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
voxel_layer:
|
||||
plugin: "nav2_costmap_2d::VoxelLayer"
|
||||
enabled: True
|
||||
publish_voxel_map: True
|
||||
origin_z: 0.0
|
||||
z_resolution: 0.05
|
||||
z_voxels: 16
|
||||
max_obstacle_height: 2.0
|
||||
mark_threshold: 0
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
always_send_full_costmap: True
|
||||
|
||||
global_costmap:
|
||||
global_costmap:
|
||||
ros__parameters:
|
||||
update_frequency: 1.0
|
||||
publish_frequency: 1.0
|
||||
global_frame: map
|
||||
robot_base_frame: base_link
|
||||
use_sim_time: True
|
||||
robot_radius: 0.22
|
||||
resolution: 0.05
|
||||
track_unknown_space: true
|
||||
plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
|
||||
obstacle_layer:
|
||||
plugin: "nav2_costmap_2d::ObstacleLayer"
|
||||
enabled: True
|
||||
observation_sources: scan
|
||||
scan:
|
||||
topic: /scan
|
||||
max_obstacle_height: 2.0
|
||||
clearing: True
|
||||
marking: True
|
||||
data_type: "LaserScan"
|
||||
raytrace_max_range: 3.0
|
||||
raytrace_min_range: 0.0
|
||||
obstacle_max_range: 2.5
|
||||
obstacle_min_range: 0.0
|
||||
static_layer:
|
||||
plugin: "nav2_costmap_2d::StaticLayer"
|
||||
map_subscribe_transient_local: True
|
||||
inflation_layer:
|
||||
plugin: "nav2_costmap_2d::InflationLayer"
|
||||
cost_scaling_factor: 3.0
|
||||
inflation_radius: 0.55
|
||||
always_send_full_costmap: True
|
||||
|
||||
planner_server:
|
||||
ros__parameters:
|
||||
expected_planner_frequency: 20.0
|
||||
use_sim_time: True
|
||||
planner_plugins: ["GridBased"]
|
||||
GridBased:
|
||||
plugin: "nav2_navfn_planner/NavfnPlanner"
|
||||
tolerance: 0.5
|
||||
use_astar: false
|
||||
allow_unknown: true
|
||||
|
||||
smoother_server:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoother_plugins: ["simple_smoother"]
|
||||
simple_smoother:
|
||||
plugin: "nav2_smoother::SimpleSmoother"
|
||||
tolerance: 1.0e-10
|
||||
max_its: 1000
|
||||
do_refinement: True
|
||||
|
||||
behavior_server:
|
||||
ros__parameters:
|
||||
costmap_topic: local_costmap/costmap_raw
|
||||
footprint_topic: local_costmap/published_footprint
|
||||
cycle_frequency: 10.0
|
||||
behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
|
||||
spin:
|
||||
plugin: "nav2_behaviors/Spin"
|
||||
backup:
|
||||
plugin: "nav2_behaviors/BackUp"
|
||||
drive_on_heading:
|
||||
plugin: "nav2_behaviors/DriveOnHeading"
|
||||
wait:
|
||||
plugin: "nav2_behaviors/Wait"
|
||||
assisted_teleop:
|
||||
plugin: "nav2_behaviors/AssistedTeleop"
|
||||
global_frame: icp_odom
|
||||
robot_base_frame: base_link
|
||||
transform_tolerance: 0.1
|
||||
use_sim_time: true
|
||||
simulate_ahead_time: 2.0
|
||||
max_rotational_vel: 1.0
|
||||
min_rotational_vel: 0.4
|
||||
rotational_acc_lim: 3.2
|
||||
|
||||
robot_state_publisher:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
|
||||
waypoint_follower:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
loop_rate: 20
|
||||
stop_on_failure: false
|
||||
waypoint_task_executor_plugin: "wait_at_waypoint"
|
||||
wait_at_waypoint:
|
||||
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
|
||||
enabled: True
|
||||
waypoint_pause_duration: 200
|
||||
|
||||
velocity_smoother:
|
||||
ros__parameters:
|
||||
use_sim_time: True
|
||||
smoothing_frequency: 20.0
|
||||
scale_velocities: False
|
||||
feedback: "OPEN_LOOP"
|
||||
max_velocity: [0.26, 0.0, 1.0]
|
||||
min_velocity: [-0.26, 0.0, -1.0]
|
||||
max_accel: [2.5, 0.0, 3.2]
|
||||
max_decel: [-2.5, 0.0, -3.2]
|
||||
odom_topic: "odom"
|
||||
odom_duration: 0.1
|
||||
deadband_velocity: [0.0, 0.0, 0.0]
|
||||
velocity_timeout: 1.0
|
||||
Reference in New Issue
Block a user