mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-03 16:27:46 +08:00
Merge branch 'ros2' of github.com:introlab/rtabmap_ros into jazzy-devel
This commit is contained in:
matlabbe
@@ -7,5 +7,6 @@
|
||||
},
|
||||
"workspaceMount": "source=${localWorkspaceFolder},target=/ros2_ws/src/rtabmap_ros,type=bind",
|
||||
"workspaceFolder": "/ros2_ws",
|
||||
"postAttachCommand": "echo 'Initialize colcon: source /opt/ros/humble/setup.bash && cd /ros2_ws && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release'"
|
||||
"postAttachCommand": "echo 'Initialize colcon: source /opt/ros/humble/setup.bash && cd /ros2_ws && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release'",
|
||||
"runArgs": ["--privileged"]
|
||||
}
|
||||
|
||||
@@ -7,5 +7,6 @@
|
||||
},
|
||||
"workspaceMount": "source=${localWorkspaceFolder},target=/ros2_ws/src/rtabmap_ros,type=bind",
|
||||
"workspaceFolder": "/ros2_ws",
|
||||
"postAttachCommand": "echo 'Initialize colcon: source /opt/ros/jazzy/setup.bash && cd /ros2_ws && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release'"
|
||||
"postAttachCommand": "echo 'Initialize colcon: source /opt/ros/jazzy/setup.bash && cd /ros2_ws && colcon build --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_BUILD_TYPE=Release'",
|
||||
"runArgs": ["--privileged"]
|
||||
}
|
||||
|
||||
@@ -11,7 +11,7 @@ jobs:
|
||||
|
||||
strategy:
|
||||
matrix:
|
||||
docker_tag: [humble, humble-latest, iron, iron-latest, jazzy-latest]
|
||||
docker_tag: [humble, humble-latest, jazzy, jazzy-latest]
|
||||
include:
|
||||
- docker_tag: humble
|
||||
docker_path: 'humble'
|
||||
@@ -22,19 +22,11 @@ jobs:
|
||||
docker_platforms: |
|
||||
linux/amd64
|
||||
linux/arm64
|
||||
- docker_tag: iron
|
||||
docker_path: 'iron'
|
||||
- docker_tag: jazzy
|
||||
docker_path: 'jazzy'
|
||||
docker_platforms: |
|
||||
linux/amd64
|
||||
- docker_tag: iron-latest
|
||||
docker_path: 'iron/latest'
|
||||
docker_platforms: |
|
||||
linux/amd64
|
||||
# Re-add "jazzy" after binaries are released
|
||||
# - docker_tag: jazzy
|
||||
# docker_path: 'jazzy'
|
||||
# docker_platforms: |
|
||||
# linux/amd64
|
||||
linux/arm64
|
||||
- docker_tag: jazzy-latest
|
||||
docker_path: 'jazzy/latest'
|
||||
docker_platforms: |
|
||||
|
||||
@@ -21,15 +21,10 @@ jobs:
|
||||
runs-on: ubuntu-latest
|
||||
strategy:
|
||||
matrix:
|
||||
ros_distro: [humble, iron]
|
||||
ros_distro: [humble]
|
||||
include:
|
||||
- ros_distro: 'humble'
|
||||
ubuntu_distro: 'jammy'
|
||||
- ros_distro: 'iron'
|
||||
ubuntu_distro: 'jammy'
|
||||
# Disabled as there still missing dependencies on jazzy:
|
||||
# - ros_distro: 'jazzy'
|
||||
# ubuntu_distro: 'noble'
|
||||
fail-fast: false
|
||||
container:
|
||||
image: rostooling/setup-ros-docker:ubuntu-${{ matrix.ubuntu_distro }}-ros-${{ matrix.ros_distro }}-desktop-latest
|
||||
|
||||
@@ -1,2 +1,3 @@
|
||||
.pydevproject
|
||||
.settings
|
||||
__pycache__
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
rtabmap_ros
|
||||
===========
|
||||
|
||||
RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Foxy minimum required**: currently most nodes are ported to ROS2, however they are not all tested yet. The interface is the same than on ROS1 (parameters and topic names should still match ROS1 documentation on [rtabmap_ros](http://wiki.ros.org/rtabmap_ros)).
|
||||
RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Foxy minimum required**: currently most nodes are ported to ROS2. The interface is the same than on ROS1 (parameters and topic names should still match ROS1 documentation on [rtabmap_ros](http://wiki.ros.org/rtabmap_ros)).
|
||||
|
||||
#### CI Latest
|
||||
|
||||
@@ -30,7 +30,7 @@ RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Foxy minimum required**: current
|
||||
<td><a href="http://build.ros.org/job/Nbin_ufv8_uFv8__rtabmap_ros__ubuntu_focal_arm64__binary/"><img src="http://build.ros.org/buildStatus/icon?job=Nbin_ufv8_uFv8__rtabmap_ros__ubuntu_focal_arm64__binary" alt="Build Status"/></td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td rowspan="3">ROS 2</td>
|
||||
<td rowspan="4">ROS 2</td>
|
||||
<td>Humble</td>
|
||||
<td><a href="http://build.ros2.org/job/Hbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
|
||||
</tr>
|
||||
@@ -38,9 +38,13 @@ RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Foxy minimum required**: current
|
||||
<td>Iron</td>
|
||||
<td><a href="http://build.ros2.org/job/Ibin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Jazzy</td>
|
||||
<td><a href="http://build.ros2.org/job/Jbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Jbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary" alt="Build Status"/></td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Rolling</td>
|
||||
<td><a href="http://build.ros2.org/job/Rbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
|
||||
<td><a href="http://build.ros2.org/job/Rbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Rbin_uN64__rtabmap_ros__ubuntu_noble_amd64__binary" alt="Build Status"/></td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td>Docker</td>
|
||||
@@ -54,41 +58,24 @@ RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Foxy minimum required**: current
|
||||
|
||||
# Usage
|
||||
|
||||
`rtabmap.launch` is also ported to ROS2 with same arguments. If you see [ROS1 examples](http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping) like this:
|
||||
* For sensor integration examples (stereo and RGB-D cameras, 3D LiDAR), see [rtabmap_examples](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) sub-folder.
|
||||
|
||||
* For robot integration examples (turtlebot3 and turtlebot4, nav2 integration), see [rtabmap_demos](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_demos) sub-folder.
|
||||
|
||||
## Logging
|
||||
To make RTAB-Map's logs appear ordered with RCLCPP's logs, set the following environment variables in your `.bashrc` (see official "[About Logging](https://docs.ros.org/en/humble/Concepts/Intermediate/About-Logging.html)" documentation for more info):
|
||||
```bash
|
||||
roslaunch zed_wrapper zed_no_tf.launch
|
||||
|
||||
roslaunch rtabmap_ros rtabmap.launch \
|
||||
rtabmap_args:="--delete_db_on_start" \
|
||||
rgb_topic:=/zed/zed_node/rgb/image_rect_color \
|
||||
depth_topic:=/zed/zed_node/depth/depth_registered \
|
||||
camera_info_topic:=/zed/zed_node/rgb/camera_info \
|
||||
frame_id:=base_link \
|
||||
approx_sync:=false \
|
||||
wait_imu_to_init:=true \
|
||||
imu_topic:=/zed_node/imu/data
|
||||
|
||||
export RCUTILS_LOGGING_USE_STDOUT=1
|
||||
export RCUTILS_LOGGING_BUFFERED_STREAM=1
|
||||
# Optional, but if you like colored logs:
|
||||
export RCUTILS_COLORIZED_OUTPUT=1
|
||||
```
|
||||
|
||||
The ROS2 equivalent is (with those [lines](https://github.com/stereolabs/zed-ros2-wrapper/blob/b512dce6ad4565f4770273995b147122e735ca0f/zed_wrapper/config/common.yaml#L58-L60) set to false to avoid TF conflicts):
|
||||
|
||||
## Recommended DDS
|
||||
If RTAB-Map's GUI or topic frequency feel laggy (even if processing time looks fast enough), it may be caused by the DDS. I recommend to use [Cyclone DDS](https://docs.ros.org/en/foxy/Installation/DDS-Implementations/Working-with-Eclipse-CycloneDDS.html), you can try it by adding this before launching any nodes/launch files:
|
||||
```bash
|
||||
ros2 launch zed_wrapper zed.launch.py
|
||||
|
||||
ros2 launch rtabmap_launch rtabmap.launch.py \
|
||||
rtabmap_args:="--delete_db_on_start" \
|
||||
rgb_topic:=/zed/zed_node/rgb/image_rect_color \
|
||||
depth_topic:=/zed/zed_node/depth/depth_registered \
|
||||
camera_info_topic:=/zed/zed_node/rgb/camera_info \
|
||||
frame_id:=base_link \
|
||||
approx_sync:=false \
|
||||
wait_imu_to_init:=true \
|
||||
imu_topic:=/zed/zed_node/imu/data \
|
||||
qos:=1 \
|
||||
rviz:=true
|
||||
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
|
||||
```
|
||||
`qos` (Quality of Service) argument should match the published topics QoS (1=RELIABLE, 2=BEST EFFORT). ROS1 was always RELIABLE.
|
||||
|
||||
# Installation
|
||||
|
||||
@@ -117,39 +104,3 @@ sudo apt install ros-$ROS_DISTRO-rtabmap-ros
|
||||
colcon build --symlink-install --cmake-args -DRTABMAP_SYNC_MULTI_RGBD=ON -DRTABMAP_SYNC_USER_DATA=ON -DCMAKE_BUILD_TYPE=Release
|
||||
```
|
||||
|
||||
# Example with Turtlebot3
|
||||
|
||||
1. Launch Turtlebot3 simulator:
|
||||
```bash
|
||||
export TURTLEBOT3_MODEL=waffle
|
||||
ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
|
||||
|
||||
export TURTLEBOT3_MODEL=waffle
|
||||
ros2 run turtlebot3_teleop teleop_keyboard
|
||||
```
|
||||
|
||||
2. Launch RTAB-Map:
|
||||
```
|
||||
ros2 launch rtabmap_demos turtlebot3_scan.launch.py
|
||||
|
||||
# OR with rtabmap.launch.py
|
||||
ros2 launch rtabmap_launch rtabmap.launch.py \
|
||||
visual_odometry:=false \
|
||||
frame_id:=base_footprint \
|
||||
subscribe_scan:=true depth:=false \
|
||||
approx_sync:=true \
|
||||
odom_topic:=/odom \
|
||||
scan_topic:=/scan \
|
||||
qos:=2 \
|
||||
args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1" \
|
||||
use_sim_time:=true \
|
||||
rviz:=true
|
||||
```
|
||||
|
||||
3. Launch navigation (`nav2_bringup` package should be installed):
|
||||
```
|
||||
ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True
|
||||
ros2 launch nav2_bringup rviz_launch.py
|
||||
```
|
||||
|
||||
See [rtabmap_demos/launch](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_demos/launch) and [rtabmap_examples/launch](https://github.com/introlab/rtabmap_ros/tree/ros2/rtabmap_examples/launch) subfolders for some other ROS2 examples with turtlebot3 in simulation and a RGB-D camera.
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>rtabmap_conversions</name>
|
||||
<version>0.21.5</version>
|
||||
<version>0.21.9</version>
|
||||
<description>RTAB-Map's conversions package. This package can be used to convert rtabmap_msgs's msgs into RTAB-Map's library objects.</description>
|
||||
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
|
||||
<author>Mathieu Labbe</author>
|
||||
|
||||
@@ -923,7 +923,8 @@ void cameraModelToROS(
|
||||
UASSERT(model.R().empty() || model.R().total() == 9);
|
||||
if(model.R().empty())
|
||||
{
|
||||
memset(camInfo.r.data(), 0.0, 9*sizeof(double));
|
||||
cv::Mat eye = cv::Mat::eye(3,3,CV_64FC1);
|
||||
memcpy(camInfo.r.data(), eye.data, 9*sizeof(double));
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -934,6 +935,10 @@ void cameraModelToROS(
|
||||
if(model.P().empty())
|
||||
{
|
||||
memset(camInfo.p.data(), 0.0, 12*sizeof(double));
|
||||
if(!model.K_raw().empty()) {
|
||||
model.K_raw().copyTo(cv::Mat(3,4,CV_64FC1, camInfo.p.data()).colRange(0,3));
|
||||
camInfo.p.back() = 1.0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -1691,18 +1696,6 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_msgs::msg::OdomInfo & msg, b
|
||||
info.localBundleOutliers = msg.local_bundle_outliers;
|
||||
info.localBundleConstraints = msg.local_bundle_constraints;
|
||||
info.localBundleTime = msg.local_bundle_time;
|
||||
UASSERT(msg.local_bundle_models.size() == msg.local_bundle_ids.size());
|
||||
UASSERT(msg.local_bundle_models.size() == msg.local_bundle_poses.size());
|
||||
for(size_t i=0; i<msg.local_bundle_ids.size(); ++i)
|
||||
{
|
||||
std::vector<rtabmap::CameraModel> models;
|
||||
for(size_t j=0; j<msg.local_bundle_models[i].models.size(); ++j)
|
||||
{
|
||||
models.push_back(cameraModelFromROS(msg.local_bundle_models[i].models[j].camera_info, transformFromGeometryMsg(msg.local_bundle_models[i].models[j].local_transform)));
|
||||
}
|
||||
info.localBundleModels.insert(std::make_pair(msg.local_bundle_ids[i], models));
|
||||
info.localBundlePoses.insert(std::make_pair(msg.local_bundle_ids[i], transformFromPoseMsg(msg.local_bundle_poses[i])));
|
||||
}
|
||||
info.keyFrameAdded = msg.key_frame_added;
|
||||
info.timeEstimation = msg.time_estimation;
|
||||
info.timeParticleFiltering = msg.time_particle_filtering;
|
||||
@@ -1720,6 +1713,19 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_msgs::msg::OdomInfo & msg, b
|
||||
|
||||
if(!ignoreData)
|
||||
{
|
||||
UASSERT(msg.local_bundle_models.size() == msg.local_bundle_ids.size());
|
||||
UASSERT(msg.local_bundle_models.size() == msg.local_bundle_poses.size());
|
||||
for(size_t i=0; i<msg.local_bundle_ids.size(); ++i)
|
||||
{
|
||||
std::vector<rtabmap::CameraModel> models;
|
||||
for(size_t j=0; j<msg.local_bundle_models[i].models.size(); ++j)
|
||||
{
|
||||
models.push_back(cameraModelFromROS(msg.local_bundle_models[i].models[j].camera_info, transformFromGeometryMsg(msg.local_bundle_models[i].models[j].local_transform)));
|
||||
}
|
||||
info.localBundleModels.insert(std::make_pair(msg.local_bundle_ids[i], models));
|
||||
info.localBundlePoses.insert(std::make_pair(msg.local_bundle_ids[i], transformFromPoseMsg(msg.local_bundle_poses[i])));
|
||||
}
|
||||
|
||||
UASSERT(msg.words_keys.size() == msg.words_values.size());
|
||||
for(unsigned int i=0; i<msg.words_keys.size(); ++i)
|
||||
{
|
||||
@@ -1988,12 +1994,6 @@ rtabmap::Transform getTransform(
|
||||
{
|
||||
// TF ready?
|
||||
rtabmap::Transform transform;
|
||||
std::string errString;
|
||||
if(!tfBuffer.canTransform(fromFrameId, toFrameId, tf2_ros::fromMsg(stamp), tf2::durationFromSec(waitForTransform), &errString))
|
||||
{
|
||||
UWARN("(can transform %s -> %s?) %s (wait_for_transform=%f)", fromFrameId.c_str(), toFrameId.c_str(), errString.c_str(), waitForTransform);
|
||||
return rtabmap::Transform();
|
||||
}
|
||||
try
|
||||
{
|
||||
geometry_msgs::msg::TransformStamped tmp;
|
||||
@@ -2174,7 +2174,7 @@ bool convertRGBDMsgs(
|
||||
rtabmap::Transform localTransform = rtabmap_conversions::getTransform(frameId, !imageMsgs.empty()?imageMsgs[i]->header.frame_id:cameraInfoMsgs[i].header.frame_id, stamp, listener, waitForTransform);
|
||||
if(localTransform.isNull())
|
||||
{
|
||||
UERROR("TF of received image %d at time %fs is not set!", i, stamp.seconds());
|
||||
UERROR("TF of received image for camera %d at time %fs is not set!", i, stamp.seconds());
|
||||
return false;
|
||||
}
|
||||
// sync with odometry stamp
|
||||
@@ -2593,6 +2593,24 @@ bool convertScanMsg(
|
||||
double waitForTransform,
|
||||
bool outputInFrameId)
|
||||
{
|
||||
// scan message validation check
|
||||
if(scan2dMsg.angle_increment == 0.0f) {
|
||||
UERROR("convertScanMsg: angle_increment should not be 0!");
|
||||
return false;
|
||||
}
|
||||
if(scan2dMsg.range_min > scan2dMsg.range_max) {
|
||||
UERROR("convertScanMsg: range_min (%f) should be smaller than range_max (%f)!", scan2dMsg.range_min, scan2dMsg.range_max);
|
||||
return false;
|
||||
}
|
||||
if(scan2dMsg.angle_increment > 0 && scan2dMsg.angle_max < scan2dMsg.angle_min) {
|
||||
UERROR("convertScanMsg: Angle increment (%f) should be negative if angle_min(%f) > angle_max(%f)!", scan2dMsg.angle_increment, scan2dMsg.angle_min, scan2dMsg.angle_max);
|
||||
return false;
|
||||
}
|
||||
else if (scan2dMsg.angle_increment < 0 && scan2dMsg.angle_max > scan2dMsg.angle_min) {
|
||||
UERROR("convertScanMsg: Angle increment (%f) should positive if angle_min(%f) < angle_max(%f)!", scan2dMsg.angle_increment, scan2dMsg.angle_min, scan2dMsg.angle_max);
|
||||
return false;
|
||||
}
|
||||
|
||||
// make sure the frame of the laser is updated during the whole scan time
|
||||
rtabmap::Transform tmpT = getMovingTransform(
|
||||
scan2dMsg.header.frame_id,
|
||||
|
||||
@@ -3,7 +3,7 @@ project(rtabmap_demos)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(DIRECTORY launch
|
||||
install(DIRECTORY launch config params data
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
|
||||
@@ -0,0 +1,101 @@
|
||||
# rtabmap_demos
|
||||
|
||||
- [rtabmap_demos](#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)
|
||||
+ [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
|
||||
```
|
||||
ros2 launch rtabmap_demos stereo_outdoor_demo.launch.py
|
||||
```
|
||||
|
||||
|
||||
### Indoor 2D LiDAR and RGB-D SLAM
|
||||
```
|
||||
ros2 launch rtabmap_demos robot_mapping_demo.launch.py rviz:=true rtabmap_viz:=true
|
||||
```
|
||||
|
||||
|
||||
### Multi-Session Indoor 2D LiDAR and RGB-D SLAM
|
||||
```
|
||||
ros2 launch rtabmap_demos multisession_mapping_demo.launch.py
|
||||
```
|
||||
|
||||
|
||||
### Find-Object with SLAM
|
||||
```
|
||||
ros2 launch rtabmap_demos find_object_demo.launch.py
|
||||
```
|
||||
|
||||
|
||||
### Turtlebot4 Nav2, 2D LiDAR and RGB-D SLAM
|
||||
```
|
||||
ros2 launch rtabmap_demos turtlebot4_sim_demo.launch.py
|
||||
```
|
||||
|
||||
|
||||
### Turtlebot3 Nav2 and 2D LiDAR SLAM
|
||||
```
|
||||
ros2 launch rtabmap_demos turtlebot3_sim_scan_demo.launch.py
|
||||
```
|
||||
|
||||
|
||||
### Turtlebot3 Nav2 and RGB-D SLAM
|
||||
```
|
||||
ros2 launch rtabmap_demos turtlebot3_sim_rgbd_demo.launch.py
|
||||
```
|
||||
|
||||
|
||||
### Turtlebot3 Nav2, 2D LiDAR and RGB-D SLAM
|
||||
```
|
||||
ros2 launch rtabmap_demos turtlebot3_sim_rgbd_scan_demo.launch.py
|
||||
```
|
||||
|
||||
|
||||
### Champ Quadruped Nav2, Elevation Map and VSLAM
|
||||
```
|
||||
ros2 launch rtabmap_demos champ_sim_vslam.launch.py
|
||||
```
|
||||
|
||||
|
||||
### Clearpath Husky Nav2, 2D LiDAR and RGB-D SLAM
|
||||
```
|
||||
ros2 launch rtabmap_demos husky_sim_scan2d_demo.launch.py
|
||||
```
|
||||
|
||||
|
||||
### Clearpath Husky Nav2, 3D LiDAR and RGB-D SLAM
|
||||
```
|
||||
ros2 launch rtabmap_demos husky_sim_scan3d_demo.launch.py
|
||||
```
|
||||
|
||||
|
||||
### Clearpath Husky Nav2, 3D LiDAR Assembling and RGB-D SLAM
|
||||
```
|
||||
ros2 launch rtabmap_demos husky_sim_scan3d_assemble_demo.launch.py
|
||||
```
|
||||
|
||||
|
||||
### Isaac Sim Nav2 and Stereo SLAM
|
||||
```
|
||||
ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py
|
||||
```
|
||||
|
||||
|
||||
### Isaac Sim Nav2 and RGB-D VSLAM
|
||||
```
|
||||
ros2 launch rtabmap_demos isaac_sim_vslam_demo.launch.py stereo:=false vo:=rtabmap
|
||||
```
|
||||
|
||||
@@ -0,0 +1,391 @@
|
||||
Panels:
|
||||
- Class: rviz_common/Displays
|
||||
Help Height: 0
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 627
|
||||
- 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
|
||||
az3_base_link:
|
||||
Value: true
|
||||
az3_odom:
|
||||
Value: true
|
||||
base_footprint:
|
||||
Value: true
|
||||
base_laser_link:
|
||||
Value: true
|
||||
base_link:
|
||||
Value: true
|
||||
map:
|
||||
Value: true
|
||||
odom:
|
||||
Value: true
|
||||
stereo_camera:
|
||||
Value: true
|
||||
stereo_camera_base:
|
||||
Value: true
|
||||
wheelLB_linkWheel_link:
|
||||
Value: true
|
||||
wheelLB_wheel_link:
|
||||
Value: true
|
||||
wheelLF_linkWheel_link:
|
||||
Value: true
|
||||
wheelLF_wheel_link:
|
||||
Value: true
|
||||
wheelRB_linkWheel_link:
|
||||
Value: true
|
||||
wheelRB_wheel_link:
|
||||
Value: true
|
||||
wheelRF_linkWheel_link:
|
||||
Value: true
|
||||
wheelRF_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:
|
||||
base_laser_link:
|
||||
{}
|
||||
stereo_camera_base:
|
||||
stereo_camera:
|
||||
{}
|
||||
wheelLB_linkWheel_link:
|
||||
wheelLB_wheel_link:
|
||||
{}
|
||||
wheelLF_linkWheel_link:
|
||||
wheelLF_wheel_link:
|
||||
{}
|
||||
wheelRB_linkWheel_link:
|
||||
wheelRB_wheel_link:
|
||||
{}
|
||||
wheelRF_linkWheel_link:
|
||||
wheelRF_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: 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: 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: /jn0/base_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
|
||||
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: 7.2877197265625
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Focal Shape Fixed Size: false
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.8703982830047607
|
||||
Target Frame: base_footprint
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 3.7535834312438965
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 846
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006330000003efc0100000002fb0000000800540069006d0065010000000000000633000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003c2000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1587
|
||||
X: 214
|
||||
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,101 @@
|
||||
#
|
||||
# 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_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,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_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'),
|
||||
]
|
||||
|
||||
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'),
|
||||
('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,138 @@
|
||||
#
|
||||
#
|
||||
# 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')
|
||||
|
||||
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_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.'),
|
||||
|
||||
# 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,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,112 @@
|
||||
# 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', '/data_throttled_image'),
|
||||
('depth/image', '/data_throttled_image_depth'),
|
||||
('rgb/camera_info', '/data_throttled_camera_info'),
|
||||
('scan', '/jn0/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('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
|
||||
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 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")]]),
|
||||
])
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
+31
-33
@@ -1,34 +1,15 @@
|
||||
# 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:
|
||||
# $ export TURTLEBOT3_MODEL=waffle
|
||||
# $ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
|
||||
#
|
||||
# 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
|
||||
# OR
|
||||
# $ ros2 launch rtabmap_launch rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint odom_topic:=/odom args:="-d" use_sim_time:=true rgb_topic:=/camera/image_raw depth_topic:=/camera/depth/image_raw camera_info_topic:=/camera/camera_info approx_sync:=true qos:=2
|
||||
# $ ros2 run topic_tools relay /rtabmap/map /map
|
||||
# $ ros2 launch rtabmap_demos turtlebot3_rgbd.launch.py
|
||||
#
|
||||
# Navigation (install nav2_bringup package):
|
||||
# $ ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True
|
||||
# $ ros2 launch nav2_bringup navigation_launch.py
|
||||
# $ ros2 launch nav2_bringup rviz_launch.py
|
||||
#
|
||||
# Teleop:
|
||||
@@ -43,7 +24,6 @@ from launch_ros.actions import Node
|
||||
def generate_launch_description():
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
qos = LaunchConfiguration('qos')
|
||||
localization = LaunchConfiguration('localization')
|
||||
|
||||
parameters={
|
||||
@@ -51,9 +31,13 @@ def generate_launch_description():
|
||||
'use_sim_time':use_sim_time,
|
||||
'subscribe_depth':True,
|
||||
'use_action_for_goal':True,
|
||||
'qos_image':qos,
|
||||
'qos_imu':qos,
|
||||
'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)
|
||||
}
|
||||
|
||||
@@ -69,10 +53,6 @@ def generate_launch_description():
|
||||
'use_sim_time', default_value='true',
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'qos', default_value='2',
|
||||
description='QoS used for input sensor topics'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Launch in localization mode.'),
|
||||
@@ -100,4 +80,22 @@ def generate_launch_description():
|
||||
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')]),
|
||||
])
|
||||
+35
-37
@@ -1,34 +1,15 @@
|
||||
# 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:
|
||||
# $ export TURTLEBOT3_MODEL=waffle
|
||||
# $ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
|
||||
#
|
||||
# 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_sync.launch.py
|
||||
# OR
|
||||
# $ ros2 launch rtabmap_launch rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=true approx_sync:=true approx_rgbd_sync:=false odom_topic:=/odom args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1 --Reg/Force3DoF true --Grid/RangeMin 0.2" use_sim_time:=true rgbd_sync:=true rgb_topic:=/camera/image_raw depth_topic:=/camera/depth/image_raw camera_info_topic:=/camera/camera_info qos:=2
|
||||
# $ ros2 run topic_tools relay /rtabmap/map /map
|
||||
# $ ros2 launch rtabmap_demos turtlebot3_rgbd_scan.launch.py
|
||||
#
|
||||
# Navigation (install nav2_bringup package):
|
||||
# $ ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True
|
||||
# $ ros2 launch nav2_bringup navigation_launch.py
|
||||
# $ ros2 launch nav2_bringup rviz_launch.py
|
||||
#
|
||||
# Teleop:
|
||||
@@ -44,7 +25,6 @@ from launch_ros.actions import Node
|
||||
def generate_launch_description():
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
qos = LaunchConfiguration('qos')
|
||||
localization = LaunchConfiguration('localization')
|
||||
|
||||
parameters={
|
||||
@@ -53,13 +33,17 @@ def generate_launch_description():
|
||||
'subscribe_rgbd':True,
|
||||
'subscribe_scan':True,
|
||||
'use_action_for_goal':True,
|
||||
'qos_scan':qos,
|
||||
'qos_image':qos,
|
||||
'qos_imu':qos,
|
||||
# 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)
|
||||
}
|
||||
@@ -73,13 +57,9 @@ def generate_launch_description():
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='true',
|
||||
'use_sim_time', default_value='false',
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'qos', default_value='2',
|
||||
description='QoS used for input sensor topics'),
|
||||
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Launch in localization mode.'),
|
||||
@@ -87,7 +67,7 @@ def generate_launch_description():
|
||||
# Nodes to launch
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time, 'qos':qos}],
|
||||
parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}],
|
||||
remappings=remappings),
|
||||
|
||||
# SLAM Mode:
|
||||
@@ -111,4 +91,22 @@ def generate_launch_description():
|
||||
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')]),
|
||||
])
|
||||
+53
-46
@@ -1,36 +1,32 @@
|
||||
# Requirements:
|
||||
# Install Turtlebot3 packages
|
||||
# Note that we can edit /opt/ros/$ROS_DISTRO/share/turtlebot3_gazebo/models/turtlebot_waffle/model.sdf
|
||||
# to increase min scan range from 0.12 to 0.2 to avoid having scans
|
||||
# hitting the robot itself
|
||||
# Example:
|
||||
# $ export TURTLEBOT3_MODEL=waffle
|
||||
# $ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
|
||||
#
|
||||
# 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
|
||||
# OR
|
||||
# $ ros2 launch rtabmap_launch rtabmap.launch.py visual_odometry:=false frame_id:=base_footprint subscribe_scan:=true depth:=false approx_sync:=true odom_topic:=/odom args:="-d --RGBD/NeighborLinkRefining true --Reg/Strategy 1 --Reg/Force3DoF true --Grid/RangeMin 0.2" use_sim_time:=true qos:=2
|
||||
# $ ros2 run topic_tools relay /rtabmap/map /map
|
||||
# $ ros2 launch rtabmap_demos turtlebot3_scan.launch.py
|
||||
#
|
||||
# Navigation (install nav2_bringup package):
|
||||
# $ ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True
|
||||
# $ 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.actions import DeclareLaunchArgument, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.conditions import IfCondition, UnlessCondition
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
def launch_setup(context, *args, **kwargs):
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
qos = LaunchConfiguration('qos')
|
||||
localization = LaunchConfiguration('localization')
|
||||
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',
|
||||
@@ -40,18 +36,51 @@ def generate_launch_description():
|
||||
'subscribe_scan':True,
|
||||
'approx_sync':True,
|
||||
'use_action_for_goal':True,
|
||||
'qos_scan':qos,
|
||||
'qos_imu':qos,
|
||||
'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
|
||||
@@ -59,35 +88,13 @@ def generate_launch_description():
|
||||
'use_sim_time', default_value='true',
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'qos', default_value='2',
|
||||
description='QoS used for input sensor topics'),
|
||||
|
||||
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),
|
||||
DeclareLaunchArgument(
|
||||
'icp_odometry', default_value='false',
|
||||
description='Launch ICP odometry on top of wheel odometry.'),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=[parameters],
|
||||
remappings=remappings),
|
||||
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)
|
||||
])
|
||||
@@ -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)
|
||||
])
|
||||
+3
-4
@@ -4,7 +4,7 @@
|
||||
#
|
||||
# Example:
|
||||
# 1) Launch simulator (turtlebot4, nav2 and rtabmap):
|
||||
# $ ros2 launch rtabmap_demos turtlebot4_ignition.launch.py
|
||||
# $ ros2 launch rtabmap_demos turtlebot4_sim_demo.launch.py
|
||||
#
|
||||
# 2) Click on "Play" button on bottom-left of gazebo.
|
||||
#
|
||||
@@ -50,7 +50,7 @@ def generate_launch_description():
|
||||
ignition_launch = PathJoinSubstitution(
|
||||
[pkg_turtlebot4_ignition_bringup, 'launch', 'turtlebot4_ignition.launch.py'])
|
||||
rtabmap_launch = PathJoinSubstitution(
|
||||
[pkg_rtabmap_demos, 'launch', 'turtlebot4_slam.launch.py'])
|
||||
[pkg_rtabmap_demos, 'launch', 'turtlebot4', 'turtlebot4_slam.launch.py'])
|
||||
|
||||
ignition = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([ignition_launch]),
|
||||
@@ -68,7 +68,6 @@ def generate_launch_description():
|
||||
launch_arguments=[
|
||||
('rtabmap_viz', LaunchConfiguration('rtabmap_viz')),
|
||||
('localization', LaunchConfiguration('localization')),
|
||||
('qos', '2'),
|
||||
('use_sim_time', 'true')
|
||||
]
|
||||
)
|
||||
@@ -77,4 +76,4 @@ def generate_launch_description():
|
||||
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
|
||||
return ld
|
||||
+10
-13
@@ -7,9 +7,9 @@
|
||||
# $ 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 qos:=2
|
||||
# $ 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" qos:=2 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
|
||||
# $ 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.
|
||||
#
|
||||
@@ -33,13 +33,12 @@ from launch_ros.actions import Node
|
||||
def generate_launch_description():
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
qos = LaunchConfiguration('qos')
|
||||
localization = LaunchConfiguration('localization')
|
||||
rtabmap_viz = LaunchConfiguration('rtabmap_viz')
|
||||
|
||||
icp_parameters={
|
||||
'odom_frame_id':'icp_odom',
|
||||
'guess_frame_id':'odom',
|
||||
'qos':qos
|
||||
'guess_frame_id':'odom'
|
||||
}
|
||||
|
||||
rtabmap_parameters={
|
||||
@@ -47,9 +46,6 @@ def generate_launch_description():
|
||||
'subscribe_scan':True,
|
||||
'use_action_for_goal':True,
|
||||
'odom_sensor_sync': True,
|
||||
'qos_scan':qos,
|
||||
'qos_image':qos,
|
||||
'qos_imu':qos,
|
||||
# RTAB-Map's parameters should be strings:
|
||||
'Mem/NotLinkedNodesKept':'false'
|
||||
}
|
||||
@@ -78,18 +74,18 @@ def generate_launch_description():
|
||||
'use_sim_time', default_value='false', choices=['true', 'false'],
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'qos', default_value='0',
|
||||
description='QoS used for input sensor topics'),
|
||||
|
||||
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, 'qos':qos}],
|
||||
parameters=[{'approx_sync':False, 'use_sim_time':use_sim_time}],
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
@@ -116,6 +112,7 @@ def generate_launch_description():
|
||||
remappings=remappings),
|
||||
|
||||
Node(
|
||||
condition=IfCondition(rtabmap_viz),
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=[rtabmap_parameters, shared_parameters],
|
||||
remappings=remappings),
|
||||
@@ -2,7 +2,7 @@
|
||||
<?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.21.5</version>
|
||||
<version>0.21.9</version>
|
||||
<description>RTAB-Map's demo launch files.</description>
|
||||
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
|
||||
<author>Mathieu Labbe</author>
|
||||
|
||||
@@ -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
|
||||
@@ -3,7 +3,7 @@ project(rtabmap_examples)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
||||
install(DIRECTORY launch
|
||||
install(DIRECTORY launch config
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
|
||||
@@ -0,0 +1,72 @@
|
||||
# Requirements:
|
||||
# A OAK-D camera
|
||||
# Install depthai-ros package (https://github.com/luxonis/depthai-ros)
|
||||
# Example:
|
||||
# $ ros2 launch rtabmap_examples depthai.launch.py camera_model:=OAK-D
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
parameters=[{'frame_id':'oak-d-base-frame',
|
||||
'subscribe_rgbd':True,
|
||||
'subscribe_odom_info':True,
|
||||
'approx_sync':False,
|
||||
'wait_imu_to_init':True}]
|
||||
|
||||
remappings=[('imu', '/imu/data')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch camera driver
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('depthai_examples'), 'launch'),
|
||||
'/stereo_inertial_node.launch.py']),
|
||||
launch_arguments={'depth_aligned': 'false',
|
||||
'enableRviz': 'false',
|
||||
'monoResolution': '400p'}.items(),
|
||||
),
|
||||
|
||||
# Sync right/depth/camera_info together
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=[('rgb/image', '/right/image_rect'),
|
||||
('rgb/camera_info', '/right/camera_info'),
|
||||
('depth/image', '/stereo/depth')]),
|
||||
|
||||
# Compute quaternion of the IMU
|
||||
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')]),
|
||||
|
||||
# Visual odometry
|
||||
Node(
|
||||
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings),
|
||||
|
||||
# VSLAM
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
# Visualization
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings)
|
||||
])
|
||||
@@ -88,7 +88,7 @@ def generate_launch_description():
|
||||
# Image rectification and publishing synchronized camera_info
|
||||
Node(
|
||||
package='rtabmap_util', executable='yaml_to_camera_info.py', output='screen',
|
||||
parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/launch/config/euroc_left.yaml']}],
|
||||
parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/config/euroc_left.yaml']}],
|
||||
remappings=[
|
||||
('image', '/cam0/image_raw'),
|
||||
('camera_info', 'left/camera_info')],
|
||||
@@ -96,7 +96,7 @@ def generate_launch_description():
|
||||
|
||||
Node(
|
||||
package='rtabmap_util', executable='yaml_to_camera_info.py', output='screen',
|
||||
parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/launch/config/euroc_right.yaml']}],
|
||||
parameters=[{'yaml_path': [FindPackageShare('rtabmap_examples'), '/config/euroc_right.yaml']}],
|
||||
remappings=[
|
||||
('image', '/cam1/image_raw'),
|
||||
('camera_info', 'right/camera_info')],
|
||||
|
||||
@@ -1,21 +1,20 @@
|
||||
# Requirements:
|
||||
# A Kinect for Azure
|
||||
# Install Azure_Kinect_ROS_Driver ros2 package (https://github.com/microsoft/Azure_Kinect_ROS_Driver/tree/humble)
|
||||
# To install Kinect SDK on Ubuntu 22.04, see https://github.com/microsoft/Azure-Kinect-Sensor-SDK/issues/1790#issuecomment-1531626651
|
||||
# udev rules: https://github.com/microsoft/Azure-Kinect-Sensor-SDK/blob/5f79890933e1c81e325633152b2f2799df825b8b/docs/usage.md#linux-device-setup
|
||||
# Install imu_filter_madgwick ros2 package
|
||||
# Example:
|
||||
# $ ros2 launch rtabmap_examples k4a.launch.py
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
parameters=[{
|
||||
'frame_id':'camera_base',
|
||||
'subscribe_rgbd':True,
|
||||
'subscribe_odom_info':True,
|
||||
'qos':1}]
|
||||
'subscribe_odom_info':True}]
|
||||
|
||||
remappings=[
|
||||
('imu', '/imu/data'),
|
||||
@@ -32,11 +31,9 @@ def generate_launch_description():
|
||||
Node(
|
||||
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
|
||||
parameters=[{ 'frame_id':'camera_base',
|
||||
'subscribe_odom_info':True,
|
||||
'approx_sync':True,
|
||||
'approx_sync_max_interval':0.01,
|
||||
'wait_imu_to_init':True,
|
||||
'qos':1,
|
||||
'queue_size':30,
|
||||
'keep_color':True,
|
||||
# Color image needs to be rectified,
|
||||
|
||||
@@ -12,8 +12,7 @@ def generate_launch_description():
|
||||
'frame_id':'camera_link',
|
||||
'subscribe_depth':True,
|
||||
'subscribe_odom_info':True,
|
||||
'approx_sync':True,
|
||||
'qos':1}]
|
||||
'approx_sync':True}]
|
||||
|
||||
remappings=[
|
||||
('rgb/image', '/kinect/rgb/image_raw'),
|
||||
|
||||
@@ -0,0 +1,220 @@
|
||||
# Description:
|
||||
# In this example, we keep only minimal data to do LiDAR SLAM.
|
||||
#
|
||||
# Example:
|
||||
# Launch your lidar sensor:
|
||||
# $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py
|
||||
# $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py
|
||||
#
|
||||
# If an IMU is used, make sure TF between lidar/base frame and imu is
|
||||
# already calibrated. In this example, we assume the imu topic has
|
||||
# already the orientation estimated, it not, you can use
|
||||
# imu_filter_madgwick_node (with use_mag:=false publish_tf:=false)
|
||||
# and set imu_topic to output topic of the filter.
|
||||
#
|
||||
# If a camera is used, make sure TF between lidar/base frame and camera is
|
||||
# already calibrated. To provide image data to this example, you should use
|
||||
# rtabmap_sync's rgbd_sync or stereo_sync node.
|
||||
#
|
||||
# Launch the example by adjusting the lidar topic and base frame:
|
||||
# $ ros2 launch rtabmap_examples lidar3d.launch.py lidar_topic:=/velodyne_points frame_id:=velodyne
|
||||
|
||||
from launch import LaunchDescription, LaunchContext
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def launch_setup(context: LaunchContext, *args, **kwargs):
|
||||
|
||||
frame_id = LaunchConfiguration('frame_id')
|
||||
|
||||
imu_topic = LaunchConfiguration('imu_topic')
|
||||
imu_used = imu_topic.perform(context) != ''
|
||||
|
||||
rgbd_image_topic = LaunchConfiguration('rgbd_image_topic')
|
||||
rgbd_image_used = rgbd_image_topic.perform(context) != ''
|
||||
|
||||
voxel_size = LaunchConfiguration('voxel_size')
|
||||
voxel_size_value = float(voxel_size.perform(context))
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
|
||||
lidar_topic = LaunchConfiguration('lidar_topic')
|
||||
lidar_topic_value = lidar_topic.perform(context)
|
||||
lidar_topic_deskewed = lidar_topic_value + "/deskewed"
|
||||
|
||||
localization = LaunchConfiguration('localization').perform(context)
|
||||
localization = localization == 'true' or localization == 'True'
|
||||
|
||||
fixed_frame_from_imu = False
|
||||
fixed_frame_id = LaunchConfiguration('fixed_frame_id').perform(context)
|
||||
if not fixed_frame_id and imu_used:
|
||||
fixed_frame_from_imu = True
|
||||
fixed_frame_id = frame_id.perform(context) + "_stabilized"
|
||||
|
||||
if not fixed_frame_id:
|
||||
lidar_topic_deskewed = lidar_topic
|
||||
|
||||
# Rule of thumb:
|
||||
max_correspondence_distance = voxel_size_value * 10.0
|
||||
|
||||
shared_parameters = {
|
||||
'use_sim_time': use_sim_time,
|
||||
'frame_id': frame_id,
|
||||
'qos': LaunchConfiguration('qos'),
|
||||
'approx_sync': rgbd_image_used,
|
||||
'wait_for_transform': 0.2,
|
||||
# RTAB-Map's internal parameters are strings:
|
||||
'Icp/PointToPlane': 'true',
|
||||
'Icp/Iterations': '10',
|
||||
'Icp/VoxelSize': str(voxel_size_value),
|
||||
'Icp/Epsilon': '0.001',
|
||||
'Icp/PointToPlaneK': '20',
|
||||
'Icp/PointToPlaneRadius': '0',
|
||||
'Icp/MaxTranslation': '3',
|
||||
'Icp/MaxCorrespondenceDistance': str(max_correspondence_distance),
|
||||
'Icp/Strategy': '1',
|
||||
'Icp/OutlierRatio': '0.7',
|
||||
}
|
||||
|
||||
icp_odometry_parameters = {
|
||||
'expected_update_rate': 15.0,
|
||||
'deskewing': not fixed_frame_id, # If fixed_frame_id is set, we do deskewing externally below
|
||||
'odom_frame_id': 'icp_odom',
|
||||
'guess_frame_id': fixed_frame_id,
|
||||
# RTAB-Map's internal parameters are strings:
|
||||
'Odom/ScanKeyFrameThr': '0.4',
|
||||
'OdomF2M/ScanSubtractRadius': str(voxel_size_value),
|
||||
'OdomF2M/ScanMaxSize': '15000',
|
||||
'OdomF2M/BundleAdjustment': 'false',
|
||||
'Icp/CorrespondenceRatio': '0.01'
|
||||
}
|
||||
if imu_used:
|
||||
icp_odometry_parameters['wait_imu_to_init'] = True
|
||||
|
||||
rtabmap_parameters = {
|
||||
'subscribe_depth': False,
|
||||
'subscribe_rgb': False,
|
||||
'subscribe_odom_info': True,
|
||||
'subscribe_scan_cloud': True,
|
||||
# RTAB-Map's internal parameters are strings:
|
||||
'RGBD/ProximityMaxGraphDepth': '0',
|
||||
'RGBD/ProximityPathMaxNeighbors': '1',
|
||||
'RGBD/AngularUpdate': '0.05',
|
||||
'RGBD/LinearUpdate': '0.05',
|
||||
'RGBD/CreateOccupancyGrid': 'false',
|
||||
'Mem/NotLinkedNodesKept': 'false',
|
||||
'Mem/STMSize': '30',
|
||||
'Reg/Strategy': '1',
|
||||
'Icp/CorrespondenceRatio': '0.2'
|
||||
}
|
||||
|
||||
arguments = []
|
||||
if localization:
|
||||
rtabmap_parameters['Mem/IncrementalMemory'] = 'False'
|
||||
rtabmap_parameters['Mem/InitWMWithAllNodes'] = 'True'
|
||||
else:
|
||||
arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db)
|
||||
|
||||
remappings = [('odom', 'icp_odom')]
|
||||
if imu_used:
|
||||
remappings.append(('imu', LaunchConfiguration('imu_topic')))
|
||||
else:
|
||||
remappings.append(('imu', 'imu_not_used'))
|
||||
if rgbd_image_used:
|
||||
remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic')))
|
||||
|
||||
nodes = [
|
||||
Node(
|
||||
package='rtabmap_odom', executable='icp_odometry', output='screen',
|
||||
parameters=[shared_parameters, icp_odometry_parameters],
|
||||
remappings=remappings + [('scan_cloud', lidar_topic_deskewed)]),
|
||||
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[shared_parameters, rtabmap_parameters, {'subscribe_rgbd': rgbd_image_used}],
|
||||
remappings=remappings + [('scan_cloud', lidar_topic_deskewed)],
|
||||
arguments=arguments),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=[shared_parameters, rtabmap_parameters],
|
||||
remappings=remappings + [('scan_cloud', 'odom_filtered_input_scan')])
|
||||
]
|
||||
|
||||
if fixed_frame_from_imu:
|
||||
# Create a stabilized base frame based on imu for lidar deskewing
|
||||
nodes.append(
|
||||
Node(
|
||||
package='rtabmap_util', executable='imu_to_tf', output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': use_sim_time,
|
||||
'fixed_frame_id': fixed_frame_id,
|
||||
'base_frame_id': frame_id,
|
||||
'wait_for_transform_duration': 0.001}],
|
||||
remappings=[('imu/data', imu_topic)]))
|
||||
|
||||
if fixed_frame_id:
|
||||
# Lidar deskewing
|
||||
nodes.append(
|
||||
Node(
|
||||
package='rtabmap_util', executable='lidar_deskewing', output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': use_sim_time,
|
||||
'fixed_frame_id': fixed_frame_id,
|
||||
'wait_for_transform': 0.2}],
|
||||
remappings=[
|
||||
('input_cloud', lidar_topic)
|
||||
])
|
||||
)
|
||||
|
||||
return nodes
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='false',
|
||||
description='Use simulated clock.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'deskewing', default_value='true',
|
||||
description='Enable lidar deskewing.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'frame_id', default_value='velodyne',
|
||||
description='Base frame of the robot.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'fixed_frame_id', default_value='',
|
||||
description='Fixed frame used for lidar deskewing. If not set, we will generate one from IMU.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Localization mode.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'lidar_topic', default_value='/velodyne_points',
|
||||
description='Name of the lidar PointCloud2 topic.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'imu_topic', default_value='',
|
||||
description='IMU topic (ignored if empty).'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'rgbd_image_topic', default_value='',
|
||||
description='RGBD image topic (ignored if empty). Would be the output of a rtabmap_sync\'s rgbd_sync, stereo_sync or rgb_sync node.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'voxel_size', default_value='0.1',
|
||||
description='Voxel size (m) of the downsampled lidar point cloud. For indoor, set it between 0.1 and 0.3. For outdoor, set it to 0.5 or over.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'qos', default_value='1',
|
||||
description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'),
|
||||
|
||||
OpaqueFunction(function=launch_setup),
|
||||
])
|
||||
|
||||
|
||||
@@ -0,0 +1,226 @@
|
||||
# Description:
|
||||
# In this example, we will record ALL lidar scans. An IMU or low latency odometry is required for this example.
|
||||
#
|
||||
# Example:
|
||||
# Launch your lidar sensor:
|
||||
# $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py
|
||||
# $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py
|
||||
#
|
||||
# Launch your IMU sensor, make sure TF between lidar/base frame and imu is already calibrated.
|
||||
# In this example, we assume the imu topic has
|
||||
# already the orientation estimated, it not, you can launch
|
||||
# imu_filter_madgwick_node (with use_mag:=false publish_tf:=false)
|
||||
# and set imu_topic to output topic of the filter.
|
||||
#
|
||||
# If a camera is used, make sure TF between lidar/base frame and camera is
|
||||
# already calibrated. To provide image data to this example, you should use
|
||||
# rtabmap_sync's rgbd_sync or stereo_sync node.
|
||||
#
|
||||
# Launch the example by adjusting the lidar topic, imu topic and base frame:
|
||||
# $ ros2 launch rtabmap_examples lidar3d.launch.py lidar_topic:=/velodyne_points imu_topic:=/imu/data frame_id:=velodyne
|
||||
|
||||
from launch import LaunchDescription, LaunchContext
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def launch_setup(context: LaunchContext, *args, **kwargs):
|
||||
|
||||
frame_id = LaunchConfiguration('frame_id')
|
||||
|
||||
fixed_frame_from_imu = False
|
||||
fixed_frame_id = LaunchConfiguration('fixed_frame_id').perform(context)
|
||||
if not fixed_frame_id:
|
||||
fixed_frame_from_imu = True
|
||||
fixed_frame_id = frame_id.perform(context) + "_stabilized"
|
||||
|
||||
imu_topic = LaunchConfiguration('imu_topic')
|
||||
|
||||
rgbd_image_topic = LaunchConfiguration('rgbd_image_topic')
|
||||
rgbd_image_used = rgbd_image_topic.perform(context) != ''
|
||||
|
||||
lidar_topic = LaunchConfiguration('lidar_topic')
|
||||
lidar_topic_value = lidar_topic.perform(context)
|
||||
lidar_topic_deskewed = lidar_topic_value + "/deskewed"
|
||||
|
||||
voxel_size = LaunchConfiguration('voxel_size')
|
||||
voxel_size_value = float(voxel_size.perform(context))
|
||||
|
||||
use_sim_time = LaunchConfiguration('use_sim_time')
|
||||
|
||||
localization = LaunchConfiguration('localization').perform(context)
|
||||
localization = localization == 'true' or localization == 'True'
|
||||
|
||||
# Rule of thumb:
|
||||
max_correspondence_distance = voxel_size_value * 10.0
|
||||
|
||||
shared_parameters = {
|
||||
'use_sim_time': use_sim_time,
|
||||
'frame_id': frame_id,
|
||||
'qos': LaunchConfiguration('qos'),
|
||||
'approx_sync': rgbd_image_used,
|
||||
'wait_for_transform': 0.2,
|
||||
# RTAB-Map's internal parameters are strings:
|
||||
'Icp/PointToPlane': 'true',
|
||||
'Icp/Iterations': '10',
|
||||
'Icp/VoxelSize': str(voxel_size_value),
|
||||
'Icp/Epsilon': '0.001',
|
||||
'Icp/PointToPlaneK': '20',
|
||||
'Icp/PointToPlaneRadius': '0',
|
||||
'Icp/MaxTranslation': '3',
|
||||
'Icp/MaxCorrespondenceDistance': str(max_correspondence_distance),
|
||||
'Icp/Strategy': '1',
|
||||
'Icp/OutlierRatio': '0.7',
|
||||
}
|
||||
|
||||
icp_odometry_parameters = {
|
||||
'expected_update_rate': 15.0,
|
||||
'wait_imu_to_init': True,
|
||||
'odom_frame_id': 'icp_odom',
|
||||
'guess_frame_id': fixed_frame_id,
|
||||
# RTAB-Map's internal parameters are strings:
|
||||
'Odom/ScanKeyFrameThr': '0.4',
|
||||
'OdomF2M/ScanSubtractRadius': str(voxel_size_value),
|
||||
'OdomF2M/ScanMaxSize': '15000',
|
||||
'OdomF2M/BundleAdjustment': 'false',
|
||||
'Icp/CorrespondenceRatio': '0.01'
|
||||
}
|
||||
|
||||
rtabmap_parameters = {
|
||||
'subscribe_depth': False,
|
||||
'subscribe_rgb': False,
|
||||
'subscribe_odom_info': True,
|
||||
'subscribe_scan_cloud': True,
|
||||
'odom_sensor_sync': True, # This will adjust camera position based on difference between lidar and camera stamps.
|
||||
# RTAB-Map's internal parameters are strings:
|
||||
'Rtabmap/DetectionRate': '0', # indirectly set to 1 Hz by the assembling time below (1s)
|
||||
'RGBD/ProximityMaxGraphDepth': '0',
|
||||
'RGBD/ProximityPathMaxNeighbors': '1',
|
||||
'RGBD/AngularUpdate': '0.05',
|
||||
'RGBD/LinearUpdate': '0.05',
|
||||
'RGBD/CreateOccupancyGrid': 'false',
|
||||
'Mem/NotLinkedNodesKept': 'false',
|
||||
'Mem/STMSize': '30',
|
||||
'Reg/Strategy': '1',
|
||||
'Icp/CorrespondenceRatio': '0.2'
|
||||
}
|
||||
|
||||
remappings = [('imu', imu_topic),
|
||||
('odom', 'icp_odom')]
|
||||
if rgbd_image_used:
|
||||
remappings.append(('rgbd_image', LaunchConfiguration('rgbd_image_topic')))
|
||||
|
||||
arguments = []
|
||||
if localization:
|
||||
rtabmap_parameters['Mem/IncrementalMemory'] = 'False'
|
||||
rtabmap_parameters['Mem/InitWMWithAllNodes'] = 'True'
|
||||
else:
|
||||
arguments.append('-d') # This will delete the previous database (~/.ros/rtabmap.db)
|
||||
|
||||
nodes = [
|
||||
# Lidar deskewing
|
||||
Node(
|
||||
package='rtabmap_util', executable='lidar_deskewing', output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': use_sim_time,
|
||||
'fixed_frame_id': fixed_frame_id,
|
||||
'wait_for_transform': 0.2}],
|
||||
remappings=[
|
||||
('input_cloud', lidar_topic)
|
||||
]),
|
||||
|
||||
# Lidar odometry
|
||||
Node(
|
||||
package='rtabmap_odom', executable='icp_odometry', output='screen',
|
||||
parameters=[shared_parameters, icp_odometry_parameters],
|
||||
remappings=remappings + [('scan_cloud', lidar_topic_deskewed)]),
|
||||
|
||||
# Assemble deskewed scans based on icp odometry
|
||||
Node(
|
||||
package='rtabmap_util', executable='point_cloud_assembler', output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': use_sim_time,
|
||||
'assembling_time': LaunchConfiguration('assembling_time'),
|
||||
'fixed_frame_id': ""}], # This will make the node subscribing to icp odometry topic "odom"
|
||||
remappings=[('cloud', lidar_topic_deskewed),
|
||||
('odom', 'icp_odom')]),
|
||||
|
||||
# Update the map
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[shared_parameters, rtabmap_parameters,
|
||||
{'subscribe_rgbd': rgbd_image_used,
|
||||
'topic_queue_size': 30,
|
||||
'sync_queue_size': 20,}],
|
||||
remappings=remappings + [('scan_cloud', 'assembled_cloud')],
|
||||
arguments=arguments),
|
||||
|
||||
# Just for visualization
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=[shared_parameters, rtabmap_parameters],
|
||||
remappings=remappings + [('scan_cloud', 'odom_filtered_input_scan')])
|
||||
]
|
||||
|
||||
if fixed_frame_from_imu:
|
||||
# Create a stabilized base frame based on imu for lidar deskewing
|
||||
nodes.append(
|
||||
Node(
|
||||
package='rtabmap_util', executable='imu_to_tf', output='screen',
|
||||
parameters=[{
|
||||
'use_sim_time': use_sim_time,
|
||||
'fixed_frame_id': fixed_frame_id,
|
||||
'base_frame_id': frame_id,
|
||||
'wait_for_transform_duration': 0.001}],
|
||||
remappings=[('imu/data', imu_topic)]))
|
||||
|
||||
return nodes
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='false',
|
||||
description='Use simulated clock.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'frame_id', default_value='velodyne',
|
||||
description='Base frame of the robot.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'fixed_frame_id', default_value='',
|
||||
description='Fixed frame used for lidar deskewing. If not set, we will generate one from IMU.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Localization mode.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'lidar_topic', default_value='/velodyne_points',
|
||||
description='Name of the lidar PointCloud2 topic.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'imu_topic', default_value='/imu/data',
|
||||
description='Name of an IMU topic.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'rgbd_image_topic', default_value='',
|
||||
description='RGBD image topic (ignored if empty). Would be the output of a rtabmap_sync\'s rgbd_sync, stereo_sync or rgb_sync node.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'voxel_size', default_value='0.1',
|
||||
description='Voxel size (m) of the downsampled lidar point cloud. For indoor, set it between 0.1 and 0.3. For outdoor, set it to 0.5 or over.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'assembling_time', default_value='1.0',
|
||||
description='How much time (sec) we assemble lidar scans before sending them to mapping node.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'qos', default_value='1',
|
||||
description='Quality of Service: 0=system default, 1=reliable, 2=best effort.'),
|
||||
|
||||
OpaqueFunction(function=launch_setup),
|
||||
])
|
||||
|
||||
|
||||
@@ -2,16 +2,16 @@
|
||||
# A realsense D400 series
|
||||
# Install realsense2 ros2 package (make sure you have this patch: https://github.com/IntelRealSense/realsense-ros/issues/2564#issuecomment-1336288238)
|
||||
# Example:
|
||||
# $ ros2 launch realsense2_camera rs_launch.py align_depth.enable:=true
|
||||
#
|
||||
# $ ros2 launch rtabmap_examples realsense_d400.launch.py
|
||||
# OR
|
||||
# $ ros2 launch rtabmap_launch rtabmap.launch.py frame_id:=camera_link args:="-d" rgb_topic:=/camera/color/image_raw depth_topic:=/camera/aligned_depth_to_color/image_raw camera_info_topic:=/camera/color/camera_info approx_sync:=false
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch_ros.actions import Node, SetParameter
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
|
||||
def generate_launch_description():
|
||||
parameters=[{
|
||||
@@ -27,7 +27,18 @@ def generate_launch_description():
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Nodes to launch
|
||||
# Make sure IR emitter is enabled
|
||||
SetParameter(name='depth_module.emitter_enabled', value=1),
|
||||
|
||||
# Launch camera driver
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('realsense2_camera'), 'launch'),
|
||||
'/rs_launch.py']),
|
||||
launch_arguments={'align_depth.enable': 'true',
|
||||
'rgb_camera.profile': '640x360x30'}.items(),
|
||||
),
|
||||
|
||||
Node(
|
||||
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
|
||||
parameters=parameters,
|
||||
|
||||
@@ -2,14 +2,17 @@
|
||||
# A realsense D435i
|
||||
# Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
|
||||
# Example:
|
||||
# $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_sync:=true
|
||||
#
|
||||
# $ ros2 launch rtabmap_examples realsense_d435i_color.launch.py
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||
from launch_ros.actions import Node, SetParameter
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
parameters=[{
|
||||
@@ -23,11 +26,32 @@ def generate_launch_description():
|
||||
('imu', '/imu/data'),
|
||||
('rgb/image', '/camera/color/image_raw'),
|
||||
('rgb/camera_info', '/camera/color/camera_info'),
|
||||
('depth/image', '/camera/realigned_depth_to_color/image_raw')]
|
||||
('depth/image', '/camera/aligned_depth_to_color/image_raw')]
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Nodes to launch
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'unite_imu_method', default_value='2',
|
||||
description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'),
|
||||
|
||||
# Make sure IR emitter is enabled
|
||||
SetParameter(name='depth_module.emitter_enabled', value=1),
|
||||
|
||||
# Launch camera driver
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('realsense2_camera'), 'launch'),
|
||||
'/rs_launch.py']),
|
||||
launch_arguments={'camera_namespace': '',
|
||||
'enable_gyro': 'true',
|
||||
'enable_accel': 'true',
|
||||
'unite_imu_method': LaunchConfiguration('unite_imu_method'),
|
||||
'align_depth.enable': 'true',
|
||||
'enable_sync': 'true',
|
||||
'rgb_camera.profile': '640x360x30'}.items(),
|
||||
),
|
||||
|
||||
Node(
|
||||
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
|
||||
parameters=parameters,
|
||||
@@ -43,26 +67,7 @@ def generate_launch_description():
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings),
|
||||
|
||||
# Because of this issue: https://github.com/IntelRealSense/realsense-ros/issues/2564
|
||||
# Generate point cloud from not aligned depth
|
||||
Node(
|
||||
package='rtabmap_util', executable='point_cloud_xyz', output='screen',
|
||||
parameters=[{'approx_sync':False}],
|
||||
remappings=[('depth/image', '/camera/depth/image_rect_raw'),
|
||||
('depth/camera_info', '/camera/depth/camera_info'),
|
||||
('cloud', '/camera/cloud_from_depth')]),
|
||||
|
||||
# Generate aligned depth to color camera from the point cloud above
|
||||
Node(
|
||||
package='rtabmap_util', executable='pointcloud_to_depthimage', output='screen',
|
||||
parameters=[{ 'decimation':2,
|
||||
'fixed_frame_id':'camera_link',
|
||||
'fill_holes_size':1}],
|
||||
remappings=[('camera_info', '/camera/color/camera_info'),
|
||||
('cloud', '/camera/cloud_from_depth'),
|
||||
('image_raw', '/camera/realigned_depth_to_color/image_raw')]),
|
||||
|
||||
|
||||
# Compute quaternion of the IMU
|
||||
Node(
|
||||
package='imu_filter_madgwick', executable='imu_filter_madgwick_node', output='screen',
|
||||
@@ -70,9 +75,4 @@ def generate_launch_description():
|
||||
'world_frame':'enu',
|
||||
'publish_tf':False}],
|
||||
remappings=[('imu/data_raw', '/camera/imu')]),
|
||||
|
||||
# The IMU frame is missing in TF tree, add it:
|
||||
Node(
|
||||
package='tf2_ros', executable='static_transform_publisher', output='screen',
|
||||
arguments=['0', '0', '0', '0', '0', '0', 'camera_gyro_optical_frame', 'camera_imu_optical_frame']),
|
||||
])
|
||||
|
||||
@@ -2,15 +2,18 @@
|
||||
# A realsense D435i
|
||||
# Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
|
||||
# Example:
|
||||
# $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_infra1:=true enable_infra2:=true enable_sync:=true
|
||||
# $ ros2 param set /camera/camera depth_module.emitter_enabled 0
|
||||
#
|
||||
# $ ros2 launch rtabmap_examples realsense_d435i_infra.launch.py
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||
from launch_ros.actions import Node, SetParameter
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
parameters=[{
|
||||
@@ -28,7 +31,28 @@ def generate_launch_description():
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Nodes to launch
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'unite_imu_method', default_value='2',
|
||||
description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'),
|
||||
|
||||
#Hack to disable IR emitter
|
||||
SetParameter(name='depth_module.emitter_enabled', value=0),
|
||||
|
||||
# Launch camera driver
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('realsense2_camera'), 'launch'),
|
||||
'/rs_launch.py']),
|
||||
launch_arguments={'camera_namespace': '',
|
||||
'enable_gyro': 'true',
|
||||
'enable_accel': 'true',
|
||||
'unite_imu_method': LaunchConfiguration('unite_imu_method'),
|
||||
'enable_infra1': 'true',
|
||||
'enable_infra2': 'true',
|
||||
'enable_sync': 'true'}.items(),
|
||||
),
|
||||
|
||||
Node(
|
||||
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
|
||||
parameters=parameters,
|
||||
@@ -52,9 +76,4 @@ def generate_launch_description():
|
||||
'world_frame':'enu',
|
||||
'publish_tf':False}],
|
||||
remappings=[('imu/data_raw', '/camera/imu')]),
|
||||
|
||||
# The IMU frame is missing in TF tree, add it:
|
||||
Node(
|
||||
package='tf2_ros', executable='static_transform_publisher', output='screen',
|
||||
arguments=['0', '0', '0', '0', '0', '0', 'camera_gyro_optical_frame', 'camera_imu_optical_frame']),
|
||||
])
|
||||
|
||||
@@ -2,15 +2,18 @@
|
||||
# A realsense D435i
|
||||
# Install realsense2 ros2 package (ros-$ROS_DISTRO-realsense2-camera)
|
||||
# Example:
|
||||
# $ ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1 enable_infra1:=true enable_infra2:=true enable_sync:=true
|
||||
# $ ros2 param set /camera/camera depth_module.emitter_enabled 0
|
||||
#
|
||||
# $ ros2 launch rtabmap_examples realsense_d435i_stereo.launch.py
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
|
||||
from launch_ros.actions import Node, SetParameter
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
def generate_launch_description():
|
||||
parameters=[{
|
||||
@@ -28,7 +31,28 @@ def generate_launch_description():
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Nodes to launch
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'unite_imu_method', default_value='2',
|
||||
description='0-None, 1-copy, 2-linear_interpolation. Use unite_imu_method:="1" if imu topics stop being published.'),
|
||||
|
||||
#Hack to disable IR emitter
|
||||
SetParameter(name='depth_module.emitter_enabled', value=0),
|
||||
|
||||
# Launch camera driver
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('realsense2_camera'), 'launch'),
|
||||
'/rs_launch.py']),
|
||||
launch_arguments={'camera_namespace': '',
|
||||
'enable_gyro': 'true',
|
||||
'enable_accel': 'true',
|
||||
'unite_imu_method': LaunchConfiguration('unite_imu_method'),
|
||||
'enable_infra1': 'true',
|
||||
'enable_infra2': 'true',
|
||||
'enable_sync': 'true'}.items(),
|
||||
),
|
||||
|
||||
Node(
|
||||
package='rtabmap_odom', executable='stereo_odometry', output='screen',
|
||||
parameters=parameters,
|
||||
@@ -52,9 +76,4 @@ def generate_launch_description():
|
||||
'world_frame':'enu',
|
||||
'publish_tf':False}],
|
||||
remappings=[('imu/data_raw', '/camera/imu')]),
|
||||
|
||||
# The IMU frame is missing in TF tree, add it:
|
||||
Node(
|
||||
package='tf2_ros', executable='static_transform_publisher', output='screen',
|
||||
arguments=['0', '0', '0', '0', '0', '0', 'camera_gyro_optical_frame', 'camera_imu_optical_frame']),
|
||||
])
|
||||
|
||||
@@ -29,7 +29,7 @@ from ament_index_python.packages import get_package_share_directory
|
||||
def generate_launch_description():
|
||||
|
||||
config_rviz = os.path.join(
|
||||
get_package_share_directory('rtabmap_examples'), 'launch', 'config', 'slam_D405x2_config.rviz')
|
||||
get_package_share_directory('rtabmap_examples'), 'config', 'slam_D405x2_config.rviz')
|
||||
|
||||
rviz_node = launch_ros.actions.Node(
|
||||
package='rviz2', executable='rviz2', output='screen',
|
||||
|
||||
@@ -31,7 +31,7 @@ from ament_index_python.packages import get_package_share_directory
|
||||
def generate_launch_description():
|
||||
|
||||
config_rviz = os.path.join(
|
||||
get_package_share_directory('rtabmap_examples'), 'launch', 'config', 'slam_D405x3_config.rviz')
|
||||
get_package_share_directory('rtabmap_examples'), 'config', 'slam_D405x3_config.rviz')
|
||||
|
||||
rviz_node = launch_ros.actions.Node(
|
||||
package='rviz2', executable='rviz2', output='screen',
|
||||
|
||||
@@ -1,126 +0,0 @@
|
||||
# Example:
|
||||
# $ ros2 launch velodyne_driver velodyne_driver_node-VLP16-launch.py
|
||||
# $ ros2 launch velodyne_pointcloud velodyne_transform_node-VLP16-launch.py
|
||||
#
|
||||
# SLAM:
|
||||
# $ ros2 launch rtabmap_examples vlp16.launch.py
|
||||
|
||||
|
||||
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')
|
||||
deskewing = LaunchConfiguration('deskewing')
|
||||
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_sim_time', default_value='false',
|
||||
description='Use simulation (Gazebo) clock if true'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'deskewing', default_value='true',
|
||||
description='Enable lidar deskewing'),
|
||||
|
||||
# Nodes to launch
|
||||
Node(
|
||||
package='rtabmap_odom', executable='icp_odometry', output='screen',
|
||||
parameters=[{
|
||||
'frame_id':'velodyne',
|
||||
'odom_frame_id':'odom',
|
||||
'wait_for_transform':0.2,
|
||||
'expected_update_rate':15.0,
|
||||
'deskewing':deskewing,
|
||||
'use_sim_time':use_sim_time,
|
||||
# RTAB-Map's internal parameters are strings:
|
||||
'Icp/PointToPlane': 'true',
|
||||
'Icp/Iterations': '10',
|
||||
'Icp/VoxelSize': '0.1',
|
||||
'Icp/Epsilon': '0.001',
|
||||
'Icp/PointToPlaneK': '20',
|
||||
'Icp/PointToPlaneRadius': '0',
|
||||
'Icp/MaxTranslation': '2',
|
||||
'Icp/MaxCorrespondenceDistance': '1',
|
||||
'Icp/Strategy': '1',
|
||||
'Icp/OutlierRatio': '0.7',
|
||||
'Icp/CorrespondenceRatio': '0.01',
|
||||
'Odom/ScanKeyFrameThr': '0.4',
|
||||
'OdomF2M/ScanSubtractRadius': '0.1',
|
||||
'OdomF2M/ScanMaxSize': '15000',
|
||||
'OdomF2M/BundleAdjustment': 'false'
|
||||
}],
|
||||
remappings=[
|
||||
('scan_cloud', '/velodyne_points')
|
||||
]),
|
||||
|
||||
Node(
|
||||
package='rtabmap_util', executable='point_cloud_assembler', output='screen',
|
||||
parameters=[{
|
||||
'max_clouds':10,
|
||||
'fixed_frame_id':'',
|
||||
'use_sim_time':use_sim_time,
|
||||
}],
|
||||
remappings=[
|
||||
('cloud', 'odom_filtered_input_scan')
|
||||
]),
|
||||
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=[{
|
||||
'frame_id':'velodyne',
|
||||
'subscribe_depth':False,
|
||||
'subscribe_rgb':False,
|
||||
'subscribe_scan_cloud':True,
|
||||
'approx_sync':False,
|
||||
'wait_for_transform':0.2,
|
||||
'use_sim_time':use_sim_time,
|
||||
# RTAB-Map's internal parameters are strings:
|
||||
'RGBD/ProximityMaxGraphDepth': '0',
|
||||
'RGBD/ProximityPathMaxNeighbors': '1',
|
||||
'RGBD/AngularUpdate': '0.05',
|
||||
'RGBD/LinearUpdate': '0.05',
|
||||
'RGBD/CreateOccupancyGrid': 'false',
|
||||
'Mem/NotLinkedNodesKept': 'false',
|
||||
'Mem/STMSize': '30',
|
||||
'Mem/LaserScanNormalK': '20',
|
||||
'Reg/Strategy': '1',
|
||||
'Icp/VoxelSize': '0.1',
|
||||
'Icp/PointToPlaneK': '20',
|
||||
'Icp/PointToPlaneRadius': '0',
|
||||
'Icp/PointToPlane': 'true',
|
||||
'Icp/Iterations': '10',
|
||||
'Icp/Epsilon': '0.001',
|
||||
'Icp/MaxTranslation': '3',
|
||||
'Icp/MaxCorrespondenceDistance': '1',
|
||||
'Icp/Strategy': '1',
|
||||
'Icp/OutlierRatio': '0.7',
|
||||
'Icp/CorrespondenceRatio': '0.2'
|
||||
}],
|
||||
remappings=[
|
||||
('scan_cloud', 'assembled_cloud')
|
||||
],
|
||||
arguments=[
|
||||
'-d' # This will delete the previous database (~/.ros/rtabmap.db)
|
||||
]),
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=[{
|
||||
'frame_id':'velodyne',
|
||||
'odom_frame_id':'odom',
|
||||
'subscribe_odom_info':True,
|
||||
'subscribe_scan_cloud':True,
|
||||
'approx_sync':False,
|
||||
'use_sim_time':use_sim_time,
|
||||
}],
|
||||
remappings=[
|
||||
('scan_cloud', 'odom_filtered_input_scan')
|
||||
]),
|
||||
])
|
||||
|
||||
|
||||
@@ -0,0 +1,121 @@
|
||||
# Example using zed odometry for lidar deskewing:
|
||||
# $ ros2 launch rtabmap_examples vlp16_zed.launch.py camera_model:=zed2i
|
||||
#
|
||||
# To use only zed's imu for deskewing:
|
||||
# $ ros2 launch rtabmap_examples vlp16_zed.launch.py camera_model:=zed2i use_zed_odometry:=false
|
||||
#
|
||||
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription, LaunchContext
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
|
||||
import tempfile
|
||||
|
||||
def launch_setup(context: LaunchContext, *args, **kwargs):
|
||||
|
||||
assemble = LaunchConfiguration('assemble').perform(context)
|
||||
assemble = assemble == 'true' or assemble == 'True'
|
||||
|
||||
lidar3d_launch_file = 'lidar3d.launch.py'
|
||||
if assemble:
|
||||
lidar3d_launch_file = 'lidar3d_assemble.launch.py'
|
||||
|
||||
use_zed_odometry = LaunchConfiguration('use_zed_odometry').perform(context)
|
||||
use_zed_odometry = use_zed_odometry == 'true' or use_zed_odometry == 'True'
|
||||
|
||||
fixed_frame_id = ''
|
||||
if use_zed_odometry:
|
||||
fixed_frame_id = 'odom'
|
||||
|
||||
# Hack to override grab_resolution parameter without changing any files
|
||||
with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file:
|
||||
zed_override_file.write("---\n"+
|
||||
"/**:\n"+
|
||||
" ros__parameters:\n"+
|
||||
" general:\n"+
|
||||
" grab_resolution: 'VGA'")
|
||||
|
||||
return [
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('velodyne_driver'), 'launch'),
|
||||
'/velodyne_driver_node-VLP16-launch.py']),
|
||||
),
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('velodyne_pointcloud'), 'launch'),
|
||||
'/velodyne_transform_node-VLP16-launch.py']),
|
||||
),
|
||||
|
||||
# Launch camera driver
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('zed_wrapper'), 'launch'),
|
||||
'/zed_camera.launch.py']),
|
||||
launch_arguments={'camera_model': LaunchConfiguration('camera_model'),
|
||||
'ros_params_override_path': zed_override_file.name,
|
||||
'publish_tf': LaunchConfiguration('use_zed_odometry'), # publish VIO frame
|
||||
'publish_map_tf': 'false'}.items(),
|
||||
),
|
||||
|
||||
# Static transform between zed and velodyne frame (zed will be our base frame because VIO is already linked to it)
|
||||
Node(package='tf2_ros', executable='static_transform_publisher', arguments=["0", "0", "-0.05", "0", "0", "0", "zed_camera_link", "velodyne"]),
|
||||
|
||||
# Sync rgb/depth/camera_info together
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
parameters=[{'approx_sync': False}],
|
||||
remappings=[('rgb/image', '/zed/zed_node/rgb/image_rect_color'),
|
||||
('rgb/camera_info', '/zed/zed_node/rgb/camera_info'),
|
||||
('depth/image', '/zed/zed_node/depth/depth_registered')]),
|
||||
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('rtabmap_examples'), 'launch'),
|
||||
'/', lidar3d_launch_file]),
|
||||
launch_arguments={'voxel_size': LaunchConfiguration('voxel_size'),
|
||||
'localization': LaunchConfiguration('localization'),
|
||||
'frame_id': 'zed_camera_link',
|
||||
'lidar_topic': 'velodyne_points',
|
||||
'imu_topic': '/zed/zed_node/imu/data',
|
||||
'rgbd_image_topic': 'rgbd_image',
|
||||
'fixed_frame_id': fixed_frame_id}.items()),
|
||||
]
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'camera_model', default_value='',
|
||||
description="[REQUIRED] The model of the camera. Using a wrong camera model can disable camera features. Valid choices are: ['zed', 'zedm', 'zed2', 'zed2i', 'zedx', 'zedxm', 'virtual']"),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'use_zed_odometry', default_value='true',
|
||||
description='Use ZED\'s odometry for deskewing.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'qos', default_value='1',
|
||||
description='Quality of Service: 0=system default, 1=reliable, 2=best effort'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'localization', default_value='false',
|
||||
description='Localization mode.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'voxel_size', default_value='0.1',
|
||||
description='Voxel size (m) of the downsampled lidar point cloud. For indoor, set it between 0.1 and 0.3. For outdoor, set it to 0.5 or over.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'assemble', default_value='false',
|
||||
description='Assemble ALL lidar scans.'),
|
||||
|
||||
OpaqueFunction(function=launch_setup),
|
||||
])
|
||||
@@ -0,0 +1,101 @@
|
||||
# Requirements:
|
||||
# A ZED camera
|
||||
# Install zed ros2 wrapper package (https://github.com/stereolabs/zed-ros2-wrapper)
|
||||
# Example:
|
||||
# $ ros2 launch rtabmap_examples zed.launch.py camera_model:=zed2i
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
from launch import LaunchDescription, LaunchContext
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch_ros.actions import Node
|
||||
from launch.actions import IncludeLaunchDescription, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.conditions import UnlessCondition
|
||||
|
||||
import tempfile
|
||||
|
||||
parameters = []
|
||||
remappings = []
|
||||
|
||||
def launch_setup(context: LaunchContext, *args, **kwargs):
|
||||
|
||||
# Hack to override grab_resolution parameter without changing any files
|
||||
with tempfile.NamedTemporaryFile(mode='w+t', delete=False) as zed_override_file:
|
||||
zed_override_file.write("---\n"+
|
||||
"/**:\n"+
|
||||
" ros__parameters:\n"+
|
||||
" general:\n"+
|
||||
" grab_resolution: 'VGA'")
|
||||
|
||||
parameters=[{'frame_id':'zed_camera_link',
|
||||
'subscribe_rgbd':True,
|
||||
'approx_sync':False,
|
||||
'wait_imu_to_init':True}]
|
||||
|
||||
remappings=[('imu', '/zed/zed_node/imu/data')]
|
||||
|
||||
if LaunchConfiguration('use_zed_odometry').perform(context) in ["True", "true"]:
|
||||
remappings.append(('odom', '/zed/zed_node/odom'))
|
||||
else:
|
||||
parameters.append({'subscribe_odom_info': True})
|
||||
|
||||
return [
|
||||
# Launch camera driver
|
||||
IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([os.path.join(
|
||||
get_package_share_directory('zed_wrapper'), 'launch'),
|
||||
'/zed_camera.launch.py']),
|
||||
launch_arguments={'camera_model': LaunchConfiguration('camera_model'),
|
||||
'ros_params_override_path': zed_override_file.name,
|
||||
'publish_tf': LaunchConfiguration('use_zed_odometry'),
|
||||
'publish_map_tf': 'false'}.items(),
|
||||
),
|
||||
|
||||
# Sync rgb/depth/camera_info together
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=[('rgb/image', '/zed/zed_node/rgb/image_rect_color'),
|
||||
('rgb/camera_info', '/zed/zed_node/rgb/camera_info'),
|
||||
('depth/image', '/zed/zed_node/depth/depth_registered')]),
|
||||
|
||||
# Visual odometry
|
||||
Node(
|
||||
package='rtabmap_odom', executable='rgbd_odometry', output='screen',
|
||||
condition=UnlessCondition(LaunchConfiguration('use_zed_odometry')),
|
||||
parameters=parameters,
|
||||
remappings=remappings,),
|
||||
|
||||
# VSLAM
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings,
|
||||
arguments=['-d']),
|
||||
|
||||
# Visualization
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', output='screen',
|
||||
parameters=parameters,
|
||||
remappings=remappings)
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
|
||||
# Launch arguments
|
||||
DeclareLaunchArgument(
|
||||
'use_zed_odometry', default_value='false',
|
||||
description='Use zed\'s computed odometry instead of using rtabmap\'s odometry.'),
|
||||
|
||||
DeclareLaunchArgument(
|
||||
'camera_model', default_value='',
|
||||
description="[REQUIRED] The model of the camera. Using a wrong camera model can disable camera features. Valid choices are: ['zed', 'zedm', 'zed2', 'zed2i', 'zedx', 'zedxm', 'virtual']"),
|
||||
|
||||
OpaqueFunction(function=launch_setup)
|
||||
])
|
||||
@@ -2,7 +2,7 @@
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>rtabmap_examples</name>
|
||||
<version>0.21.5</version>
|
||||
<version>0.21.9</version>
|
||||
<description>RTAB-Map's example launch files.</description>
|
||||
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
|
||||
<author>Mathieu Labbe</author>
|
||||
|
||||
@@ -0,0 +1,40 @@
|
||||
|
||||
|
||||
# Usage
|
||||
|
||||
`rtabmap.launch` from ros1 has been ported to ROS2 as `rtabmap.launch.py` with same arguments. If you see [ROS1 examples](http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping) like this:
|
||||
|
||||
```bash
|
||||
roslaunch zed_wrapper zed_no_tf.launch
|
||||
|
||||
roslaunch rtabmap_ros rtabmap.launch \
|
||||
rtabmap_args:="--delete_db_on_start" \
|
||||
rgb_topic:=/zed/zed_node/rgb/image_rect_color \
|
||||
depth_topic:=/zed/zed_node/depth/depth_registered \
|
||||
camera_info_topic:=/zed/zed_node/rgb/camera_info \
|
||||
frame_id:=base_link \
|
||||
approx_sync:=false \
|
||||
wait_imu_to_init:=true \
|
||||
imu_topic:=/zed_node/imu/data
|
||||
|
||||
```
|
||||
|
||||
The ROS2 equivalent is (using latest zed_wrapper launch file):
|
||||
|
||||
```bash
|
||||
ros2 launch zed_wrapper zed_camera.launch.py camera_model:=zed2i \
|
||||
publish_tf:=false \
|
||||
publish_map_tf:=false
|
||||
|
||||
ros2 launch rtabmap_launch rtabmap.launch.py \
|
||||
rtabmap_args:="--delete_db_on_start" \
|
||||
rgb_topic:=/zed/zed_node/rgb/image_rect_color \
|
||||
depth_topic:=/zed/zed_node/depth/depth_registered \
|
||||
camera_info_topic:=/zed/zed_node/rgb/camera_info \
|
||||
frame_id:=zed_camera_link \
|
||||
approx_sync:=false \
|
||||
wait_imu_to_init:=true \
|
||||
imu_topic:=/zed/zed_node/imu/data \
|
||||
rviz:=true
|
||||
```
|
||||
|
||||
@@ -85,6 +85,7 @@ def launch_setup(context, *args, **kwargs):
|
||||
namespace=LaunchConfiguration('namespace')),
|
||||
Node(
|
||||
package='rtabmap_sync', executable='rgbd_sync', name="rgbd_sync", output="screen",
|
||||
emulate_tty=True,
|
||||
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' != 'true' and '", LaunchConfiguration('rgbd_sync'), "' == 'true'"])),
|
||||
parameters=[{
|
||||
"approx_sync": LaunchConfiguration('approx_rgbd_sync'),
|
||||
@@ -120,6 +121,7 @@ def launch_setup(context, *args, **kwargs):
|
||||
namespace=LaunchConfiguration('namespace')),
|
||||
Node(
|
||||
package='rtabmap_sync', executable='stereo_sync', name="stereo_sync", output="screen",
|
||||
emulate_tty=True,
|
||||
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('stereo'), "' == 'true' and '", LaunchConfiguration('rgbd_sync'), "' == 'true'"])),
|
||||
parameters=[{
|
||||
"approx_sync": LaunchConfiguration('approx_rgbd_sync'),
|
||||
@@ -139,6 +141,7 @@ def launch_setup(context, *args, **kwargs):
|
||||
# Relay rgbd_image
|
||||
Node(
|
||||
package='rtabmap_util', executable='rgbd_relay', name="rgbd_relay", output="screen",
|
||||
emulate_tty=True,
|
||||
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('rgbd_sync'), "' != 'true' and '", LaunchConfiguration('subscribe_rgbd'), "' == 'true' and '", LaunchConfiguration('compressed'), "' != 'true'"])),
|
||||
parameters=[{
|
||||
"qos": LaunchConfiguration('qos_image')}],
|
||||
@@ -148,6 +151,7 @@ def launch_setup(context, *args, **kwargs):
|
||||
namespace=LaunchConfiguration('namespace')),
|
||||
Node(
|
||||
package='rtabmap_util', executable='rgbd_relay', name="rgbd_relay_uncompress", output="screen",
|
||||
emulate_tty=True,
|
||||
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('rgbd_sync'), "' != 'true' and '", LaunchConfiguration('subscribe_rgbd'), "' == 'true' and '", LaunchConfiguration('compressed'), "' == 'true'"])),
|
||||
parameters=[{
|
||||
"uncompress": True,
|
||||
@@ -160,6 +164,7 @@ def launch_setup(context, *args, **kwargs):
|
||||
# RGB-D odometry
|
||||
Node(
|
||||
package='rtabmap_odom', executable='rgbd_odometry', name="rgbd_odometry", output="screen",
|
||||
emulate_tty=True,
|
||||
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' != 'true' and '", LaunchConfiguration('visual_odometry'), "' == 'true' and '", LaunchConfiguration('stereo'), "' != 'true'"])),
|
||||
parameters=[{
|
||||
"frame_id": LaunchConfiguration('frame_id'),
|
||||
@@ -169,6 +174,7 @@ def launch_setup(context, *args, **kwargs):
|
||||
"ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context),
|
||||
"wait_for_transform": LaunchConfiguration('wait_for_transform'),
|
||||
"wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'),
|
||||
"always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'),
|
||||
"approx_sync": LaunchConfiguration('approx_sync'),
|
||||
"approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'),
|
||||
"config_path": LaunchConfiguration('cfg').perform(context),
|
||||
@@ -195,6 +201,7 @@ def launch_setup(context, *args, **kwargs):
|
||||
# Stereo odometry
|
||||
Node(
|
||||
package='rtabmap_odom', executable='stereo_odometry', name="stereo_odometry", output="screen",
|
||||
emulate_tty=True,
|
||||
condition=IfCondition(PythonExpression(["'", LaunchConfiguration('icp_odometry'), "' != 'true' and '", LaunchConfiguration('visual_odometry'), "' == 'true' and '", LaunchConfiguration('stereo'), "' == 'true'"])),
|
||||
parameters=[{
|
||||
"frame_id": LaunchConfiguration('frame_id'),
|
||||
@@ -204,6 +211,7 @@ def launch_setup(context, *args, **kwargs):
|
||||
"ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context),
|
||||
"wait_for_transform": LaunchConfiguration('wait_for_transform'),
|
||||
"wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'),
|
||||
"always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'),
|
||||
"approx_sync": LaunchConfiguration('approx_sync'),
|
||||
"approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'),
|
||||
"config_path": LaunchConfiguration('cfg').perform(context),
|
||||
@@ -231,6 +239,7 @@ def launch_setup(context, *args, **kwargs):
|
||||
# ICP odometry
|
||||
Node(
|
||||
package='rtabmap_odom', executable='icp_odometry', name="icp_odometry", output="screen",
|
||||
emulate_tty=True,
|
||||
condition=IfCondition(LaunchConfiguration('icp_odometry')),
|
||||
parameters=[{
|
||||
"frame_id": LaunchConfiguration('frame_id'),
|
||||
@@ -240,6 +249,7 @@ def launch_setup(context, *args, **kwargs):
|
||||
"ground_truth_base_frame_id": LaunchConfiguration('ground_truth_base_frame_id').perform(context),
|
||||
"wait_for_transform": LaunchConfiguration('wait_for_transform'),
|
||||
"wait_imu_to_init": LaunchConfiguration('wait_imu_to_init'),
|
||||
"always_check_imu_tf": LaunchConfiguration('always_check_imu_tf'),
|
||||
"approx_sync": LaunchConfiguration('approx_sync'),
|
||||
"config_path": LaunchConfiguration('cfg').perform(context),
|
||||
"topic_queue_size": LaunchConfiguration('topic_queue_size'),
|
||||
@@ -260,6 +270,7 @@ def launch_setup(context, *args, **kwargs):
|
||||
|
||||
Node(
|
||||
package='rtabmap_slam', executable='rtabmap', name="rtabmap", output="screen",
|
||||
emulate_tty=True,
|
||||
parameters=[{
|
||||
"subscribe_depth": LaunchConfiguration('depth'),
|
||||
"subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'),
|
||||
@@ -325,6 +336,7 @@ def launch_setup(context, *args, **kwargs):
|
||||
|
||||
Node(
|
||||
package='rtabmap_viz', executable='rtabmap_viz', name="rtabmap_viz", output='screen',
|
||||
emulate_tty=True,
|
||||
parameters=[{
|
||||
"subscribe_depth": LaunchConfiguration('depth'),
|
||||
"subscribe_rgbd": LaunchConfiguration('subscribe_rgbd'),
|
||||
@@ -368,12 +380,15 @@ def launch_setup(context, *args, **kwargs):
|
||||
arguments=[["-d"], [LaunchConfiguration("rviz_cfg")]]),
|
||||
Node(
|
||||
package='rtabmap_util', executable='point_cloud_xyzrgb', name="point_cloud_xyzrgb", output='screen',
|
||||
emulate_tty=True,
|
||||
condition=IfCondition(LaunchConfiguration("rviz")),
|
||||
parameters=[{
|
||||
"decimation": 4,
|
||||
"voxel_size": 0.0,
|
||||
"approx_sync": LaunchConfiguration('approx_sync'),
|
||||
"approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval')
|
||||
"approx_sync_max_interval": LaunchConfiguration('approx_sync_max_interval'),
|
||||
"qos": LaunchConfiguration('qos_image'),
|
||||
"qos_camera_info": LaunchConfiguration('qos_camera_info')
|
||||
}],
|
||||
remappings=[
|
||||
('left/image', LaunchConfiguration('left_image_topic_relay')),
|
||||
@@ -418,9 +433,9 @@ def generate_launch_description():
|
||||
DeclareLaunchArgument('publish_tf_map', default_value='true', description='Publish TF between map and odomerty.'),
|
||||
DeclareLaunchArgument('namespace', default_value='rtabmap', description=''),
|
||||
DeclareLaunchArgument('database_path', default_value='~/.ros/rtabmap.db', description='Where is the map saved/loaded.'),
|
||||
DeclareLaunchArgument('topic_queue_size', default_value='1', description='Queue size of individual topic subscribers.'),
|
||||
DeclareLaunchArgument('topic_queue_size', default_value='10', description='Queue size of individual topic subscribers.'),
|
||||
DeclareLaunchArgument('queue_size', default_value='10', description='Backward compatibility, use "sync_queue_size" instead.'),
|
||||
DeclareLaunchArgument('qos', default_value='1', description='General QoS used for sensor input data: 0=system default, 1=Reliable, 2=Best Effort.'),
|
||||
DeclareLaunchArgument('qos', default_value='0', description='General QoS used for sensor input data: 0=system default, 1=Reliable, 2=Best Effort.'),
|
||||
DeclareLaunchArgument('wait_for_transform', default_value='0.2', description=''),
|
||||
DeclareLaunchArgument('rtabmap_args', default_value='', description='Backward compatibility, use "args" instead.'),
|
||||
DeclareLaunchArgument('launch_prefix', default_value='', description='For debugging purpose, it fills prefix tag of the nodes, e.g., "xterm -e gdb -ex run --args"'),
|
||||
@@ -480,11 +495,12 @@ def generate_launch_description():
|
||||
DeclareLaunchArgument('odom_guess_frame_id', default_value='', description=''),
|
||||
DeclareLaunchArgument('odom_guess_min_translation', default_value='0.0', description=''),
|
||||
DeclareLaunchArgument('odom_guess_min_rotation', default_value='0.0', description=''),
|
||||
|
||||
|
||||
# imu
|
||||
DeclareLaunchArgument('imu_topic', default_value='/imu/data', description='Used with VIO approaches and for SLAM graph optimization (gravity constraints).'),
|
||||
DeclareLaunchArgument('wait_imu_to_init', default_value='false', description=''),
|
||||
|
||||
DeclareLaunchArgument('always_check_imu_tf', default_value='true', description='The odometry node will always check if TF between IMU frame and base frame has changed. If false, it is checked till a valid transform is initialized.'),
|
||||
|
||||
# User Data
|
||||
DeclareLaunchArgument('subscribe_user_data', default_value='false', description='User data synchronized subscription.'),
|
||||
DeclareLaunchArgument('user_data_topic', default_value='/user_data', description=''),
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>rtabmap_launch</name>
|
||||
<version>0.21.5</version>
|
||||
<version>0.21.9</version>
|
||||
<description>RTAB-Map's main launch files.</description>
|
||||
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
|
||||
<author>Mathieu Labbe</author>
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>rtabmap_msgs</name>
|
||||
<version>0.21.5</version>
|
||||
<version>0.21.9</version>
|
||||
<description>RTAB-Map's msgs package.</description>
|
||||
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
|
||||
<author>Mathieu Labbe</author>
|
||||
|
||||
@@ -47,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <rtabmap_msgs/srv/reset_pose.hpp>
|
||||
#include <rtabmap/core/SensorData.h>
|
||||
#include <rtabmap/core/Parameters.h>
|
||||
#include <rtabmap/utilite/UThread.h>
|
||||
|
||||
#include <boost/thread.hpp>
|
||||
|
||||
@@ -59,7 +60,7 @@ class Odometry;
|
||||
|
||||
namespace rtabmap_odom {
|
||||
|
||||
class OdometryROS : public rclcpp::Node
|
||||
class OdometryROS : public rclcpp::Node, public UThread
|
||||
{
|
||||
|
||||
public:
|
||||
@@ -98,12 +99,18 @@ protected:
|
||||
|
||||
private:
|
||||
|
||||
virtual void mainLoop();
|
||||
virtual void mainLoopKill();
|
||||
virtual void updateParameters(rtabmap::ParametersMap &) {}
|
||||
virtual void onOdomInit() {}
|
||||
|
||||
void callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg);
|
||||
void reset(const rtabmap::Transform & pose = rtabmap::Transform::getIdentity());
|
||||
|
||||
protected:
|
||||
rclcpp::CallbackGroup::SharedPtr dataCallbackGroup_;
|
||||
void tick(const rclcpp::Time & stamp);
|
||||
|
||||
private:
|
||||
rtabmap::Odometry * odometry_;
|
||||
|
||||
@@ -147,6 +154,14 @@ private:
|
||||
std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tfListener_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imuSub_;
|
||||
rclcpp::CallbackGroup::SharedPtr imuCallbackGroup_;
|
||||
|
||||
// Safe-threading
|
||||
UMutex imuMutex_;
|
||||
UMutex dataMutex_;
|
||||
USemaphore dataReady_;
|
||||
rtabmap::SensorData dataToProcess_;
|
||||
std_msgs::msg::Header dataHeaderToProcess_;
|
||||
|
||||
bool paused_;
|
||||
int resetCountdown_;
|
||||
@@ -164,11 +179,14 @@ private:
|
||||
bool compressionParallelized_;
|
||||
int odomStrategy_;
|
||||
bool waitIMUToinit_;
|
||||
bool alwaysCheckImuTf_;
|
||||
bool imuProcessed_;
|
||||
std::map<double, rtabmap::IMU> imus_;
|
||||
std::pair<rtabmap::SensorData, std_msgs::msg::Header > bufferedData_;
|
||||
int processedMsgs_;
|
||||
int droppedMsgs_;
|
||||
std::map<double, sensor_msgs::msg::Imu::ConstSharedPtr> imus_;
|
||||
std::string configPath_;
|
||||
rtabmap::Transform initialPose_;
|
||||
rtabmap::Transform imuLocalTransform_;
|
||||
|
||||
rtabmap_util::ULogToRosout ulogToRosout_;
|
||||
|
||||
@@ -176,11 +194,13 @@ private:
|
||||
{
|
||||
public:
|
||||
OdomStatusTask();
|
||||
void setStatus(bool isLost);
|
||||
void setStatus(bool isLost, int processedMsgs, int droppedMsgs);
|
||||
void run(diagnostic_updater::DiagnosticStatusWrapper &stat);
|
||||
private:
|
||||
bool lost_;
|
||||
bool dataReceived_;
|
||||
int processedMsgs_;
|
||||
int droppedMsgs_;
|
||||
};
|
||||
OdomStatusTask statusDiagnostic_;
|
||||
std::unique_ptr<rtabmap_sync::SyncDiagnostic> syncDiagnostic_;
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>rtabmap_odom</name>
|
||||
<version>0.21.5</version>
|
||||
<version>0.21.9</version>
|
||||
<description>RTAB-Map's odometry package.</description>
|
||||
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
|
||||
<author>Mathieu Labbe</author>
|
||||
|
||||
@@ -70,7 +70,10 @@ int main(int argc, char **argv)
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::NodeOptions options;
|
||||
options.arguments(arguments);
|
||||
rclcpp::spin(std::make_shared<rtabmap_odom::ICPOdometry>(options));
|
||||
auto node = std::make_shared<rtabmap_odom::ICPOdometry>(options);
|
||||
rclcpp::executors::MultiThreadedExecutor executor;
|
||||
executor.add_node(node);
|
||||
executor.spin();
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
|
||||
+268
-177
@@ -92,11 +92,16 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
|
||||
compressionParallelized_(true),
|
||||
odomStrategy_(Parameters::defaultOdomStrategy()),
|
||||
waitIMUToinit_(false),
|
||||
alwaysCheckImuTf_(true),
|
||||
imuProcessed_(false),
|
||||
processedMsgs_(0),
|
||||
droppedMsgs_(0),
|
||||
configPath_(),
|
||||
initialPose_(Transform::getIdentity()),
|
||||
ulogToRosout_(this)
|
||||
{
|
||||
dataCallbackGroup_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
|
||||
|
||||
int qos = this->declare_parameter("qos", (int)qos_);
|
||||
qos_ = (rmw_qos_reliability_policy_t)qos;
|
||||
|
||||
@@ -111,11 +116,7 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
|
||||
odomSensorDataFeaturesPub_ = create_publisher<rtabmap_msgs::msg::SensorData>("odom_sensor_data/features", rclcpp::QoS(1).reliability(qos_));
|
||||
odomSensorDataCompressedPub_ = create_publisher<rtabmap_msgs::msg::SensorData>("odom_sensor_data/compressed", rclcpp::QoS(1).reliability(qos_));
|
||||
|
||||
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
|
||||
//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
|
||||
// this->get_node_base_interface(),
|
||||
// this->get_node_timers_interface());
|
||||
//tfBuffer_->setCreateTimerInterface(timer_interface);
|
||||
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(get_clock());
|
||||
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
|
||||
tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
|
||||
|
||||
@@ -144,6 +145,8 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
|
||||
compressionParallelized_ = this->declare_parameter("sensor_data_parallel_compression", compressionParallelized_);
|
||||
|
||||
waitIMUToinit_ = this->declare_parameter("wait_imu_to_init", waitIMUToinit_);
|
||||
alwaysCheckImuTf_ = this->declare_parameter("always_check_imu_tf", alwaysCheckImuTf_);
|
||||
|
||||
|
||||
configPath_ = uReplaceChar(configPath_, '~', UDirectory::homeDir());
|
||||
if(configPath_.size() && configPath_.at(0) != '/')
|
||||
@@ -198,12 +201,14 @@ OdometryROS::OdometryROS(const std::string & name, const rclcpp::NodeOptions & o
|
||||
RCLCPP_INFO(this->get_logger(), "Odometry: max_update_rate = %f Hz", maxUpdateRate_);
|
||||
RCLCPP_INFO(this->get_logger(), "Odometry: min_update_rate = %f Hz", minUpdateRate_);
|
||||
RCLCPP_INFO(this->get_logger(), "Odometry: wait_imu_to_init = %s", waitIMUToinit_?"true":"false");
|
||||
RCLCPP_INFO(this->get_logger(), "Odometry: always_check_imu_tf = %s", alwaysCheckImuTf_?"true":"false");
|
||||
RCLCPP_INFO(this->get_logger(), "Odometry: sensor_data_compression_format = %s", compressionImgFormat_.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), "Odometry: sensor_data_parallel_compression = %s", compressionParallelized_?"true":"false");
|
||||
}
|
||||
|
||||
OdometryROS::~OdometryROS()
|
||||
{
|
||||
this->join(true);
|
||||
delete odometry_;
|
||||
}
|
||||
|
||||
@@ -365,14 +370,19 @@ void OdometryROS::init(bool stereoParams, bool visParams, bool icpParams)
|
||||
Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_);
|
||||
if(waitIMUToinit_)
|
||||
{
|
||||
int queueSize = 10;
|
||||
this->get_parameter_or("queue_size", queueSize, queueSize);
|
||||
imuCallbackGroup_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
|
||||
rclcpp::SubscriptionOptions options;
|
||||
options.callback_group = imuCallbackGroup_;
|
||||
int queueSize = this->declare_parameter("imu_queue_size", 200);
|
||||
int qosImu = this->declare_parameter("qos_imu", (int)qos_);
|
||||
imuSub_ = create_subscription<sensor_msgs::msg::Imu>("imu", rclcpp::QoS(queueSize*5).reliability((rmw_qos_reliability_policy_t)qosImu), std::bind(&OdometryROS::callbackIMU, this, std::placeholders::_1));
|
||||
imuSub_ = create_subscription<sensor_msgs::msg::Imu>("imu", rclcpp::QoS(queueSize).reliability((rmw_qos_reliability_policy_t)qosImu), std::bind(&OdometryROS::callbackIMU, this, std::placeholders::_1), options);
|
||||
RCLCPP_INFO(this->get_logger(), "odometry: Subscribing to IMU topic %s", imuSub_->get_topic_name());
|
||||
RCLCPP_INFO(this->get_logger(), "odometry: qos_imu = %d", qosImu);
|
||||
RCLCPP_INFO(this->get_logger(), "odometry: imu_queue_size = %d", queueSize);
|
||||
}
|
||||
|
||||
this->start();
|
||||
|
||||
onOdomInit();
|
||||
}
|
||||
|
||||
@@ -408,38 +418,26 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg)
|
||||
if(!this->isPaused())
|
||||
{
|
||||
double stamp = rtabmap_conversions::timestampFromROS(msg->header.stamp);
|
||||
rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
|
||||
if(this->frameId().compare(msg->header.frame_id) != 0)
|
||||
//RCLCPP_WARN(get_logger(), "Received imu: %f delay=%f", stamp, (now() - msg->header.stamp).seconds());
|
||||
|
||||
UScopeMutex m(imuMutex_);
|
||||
|
||||
if(!imuProcessed_ && imus_.empty())
|
||||
{
|
||||
localTransform = rtabmap_conversions::getTransform(this->frameId(), msg->header.frame_id, msg->header.stamp, *tfBuffer_, waitForTransform_);
|
||||
}
|
||||
if(localTransform.isNull())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Could not transform IMU msg from frame \"%s\" to frame \"%s\", TF not available at time %f",
|
||||
msg->header.frame_id.c_str(), this->frameId().c_str(), stamp);
|
||||
return;
|
||||
rtabmap::Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), msg->header.frame_id, msg->header.stamp, *tfBuffer_, waitForTransform_);
|
||||
if(localTransform.isNull())
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Dropping imu data! A valid TF between %s and %s is required to initialize IMU.",
|
||||
this->frameId().c_str(), msg->header.frame_id.c_str());
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
IMU imu(cv::Vec4d(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w),
|
||||
cv::Mat(3,3,CV_64FC1,(void*)msg->orientation_covariance.data()).clone(),
|
||||
cv::Vec3d(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z),
|
||||
cv::Mat(3,3,CV_64FC1,(void*)msg->angular_velocity_covariance.data()).clone(),
|
||||
cv::Vec3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z),
|
||||
cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(),
|
||||
localTransform);
|
||||
|
||||
imus_.insert(std::make_pair(stamp, imu));
|
||||
//RCLCPP_WARN(get_logger(), "Received imu: %f", stamp);
|
||||
|
||||
if(bufferedData_.first.isValid() && stamp > bufferedData_.first.stamp())
|
||||
{
|
||||
SensorData data = bufferedData_.first;
|
||||
bufferedData_.first = SensorData();
|
||||
processData(data, bufferedData_.second);
|
||||
}
|
||||
imus_.insert(std::make_pair(stamp, msg));
|
||||
|
||||
if(imus_.size() > 1000)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "Dropping imu data!");
|
||||
imus_.erase(imus_.begin());
|
||||
}
|
||||
}
|
||||
@@ -447,45 +445,117 @@ void OdometryROS::callbackIMU(const sensor_msgs::msg::Imu::SharedPtr msg)
|
||||
|
||||
void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & header)
|
||||
{
|
||||
if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && imus_.empty())
|
||||
//RCLCPP_WARN(get_logger(), "Received image: %f delay=%f", data.stamp(), (now() - header.stamp).seconds());
|
||||
if(dataMutex_.lockTry() == 0)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(), "odometry: waiting imu (%s) to initialize orientation (wait_imu_to_init=true)", imuSub_->get_topic_name());
|
||||
dataToProcess_ = data;
|
||||
dataHeaderToProcess_ = header;
|
||||
dataReady_.release();
|
||||
dataMutex_.unlock();
|
||||
++processedMsgs_;
|
||||
}
|
||||
else
|
||||
{
|
||||
//RCLCPP_WARN(get_logger(), "Dropping image/scan data");
|
||||
++droppedMsgs_;
|
||||
}
|
||||
}
|
||||
|
||||
void OdometryROS::mainLoopKill()
|
||||
{
|
||||
// in case we were waiting, unblock thread
|
||||
dataReady_.release();
|
||||
}
|
||||
|
||||
void OdometryROS::mainLoop()
|
||||
{
|
||||
dataReady_.acquire();
|
||||
|
||||
if(!this->isRunning())
|
||||
{
|
||||
// thread killed
|
||||
return;
|
||||
}
|
||||
|
||||
if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < rtabmap_conversions::timestampFromROS(header.stamp)))
|
||||
{
|
||||
//RCLCPP_WARN(get_logger(), "No imu received with higher stamp than last image (%f)! Buffering this image until we get more imu msgs...", timestampFromROS(header.stamp));
|
||||
UScopeMutex lock(dataMutex_);
|
||||
|
||||
// keep in cache to process later when we will receive imu msgs
|
||||
if(bufferedData_.first.isValid())
|
||||
// aliases
|
||||
SensorData & data = dataToProcess_;
|
||||
std_msgs::msg::Header & header = dataHeaderToProcess_;
|
||||
|
||||
std::vector<std::pair<double, sensor_msgs::msg::Imu::ConstSharedPtr> > imus;
|
||||
{
|
||||
UScopeMutex m(imuMutex_);
|
||||
|
||||
if((waitIMUToinit_ && !imuProcessed_) && odometry_->framesProcessed() == 0 && odometry_->getPose().isIdentity() && imus_.empty())
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Overwriting previous data! Make sure IMU is "
|
||||
"published faster than data rate. (last image stamp "
|
||||
"buffered=%f and new one is %f, last imu stamp received=%f)",
|
||||
bufferedData_.first.stamp(), data.stamp(), imus_.empty()?0:imus_.rbegin()->first);
|
||||
RCLCPP_WARN(this->get_logger(), "odometry: waiting imu (%s) to initialize orientation (wait_imu_to_init=true)", imuSub_->get_topic_name());
|
||||
return;
|
||||
}
|
||||
bufferedData_.first = data;
|
||||
bufferedData_.second = header;
|
||||
return;
|
||||
}
|
||||
// process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp)
|
||||
std::map<double, rtabmap::IMU>::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp));
|
||||
if(iterEnd!= imus_.end())
|
||||
|
||||
if(waitIMUToinit_ && (imus_.empty() || imus_.rbegin()->first < rtabmap_conversions::timestampFromROS(header.stamp)))
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Make sure IMU is published faster than data rate! (last image stamp=%f and last imu stamp received=%f)",
|
||||
data.stamp(), imus_.empty()?0:imus_.rbegin()->first);
|
||||
return;
|
||||
}
|
||||
// process all imu data up to current image stamp (or just after so that underlying odom approach can do interpolation of imu at image stamp)
|
||||
std::map<double, sensor_msgs::msg::Imu::ConstSharedPtr>::iterator iterEnd = imus_.lower_bound(rtabmap_conversions::timestampFromROS(header.stamp));
|
||||
if(iterEnd!= imus_.end())
|
||||
{
|
||||
++iterEnd;
|
||||
}
|
||||
for(std::map<double, sensor_msgs::msg::Imu::ConstSharedPtr>::iterator iter=imus_.begin(); iter!=iterEnd;)
|
||||
{
|
||||
imus.push_back(*iter);
|
||||
imus_.erase(iter++);
|
||||
}
|
||||
} // end imu lock
|
||||
|
||||
bool imuWarnShown = false;
|
||||
for(size_t i=0; i<imus.size(); ++i)
|
||||
{
|
||||
++iterEnd;
|
||||
}
|
||||
for(std::map<double, rtabmap::IMU>::iterator iter=imus_.begin(); iter!=iterEnd;)
|
||||
{
|
||||
//NODELET_WARN("img callback: process imu %f", iter->first);
|
||||
SensorData dataIMU(iter->second, 0, iter->first);
|
||||
if((alwaysCheckImuTf_ && !imuWarnShown) || imuLocalTransform_.isNull())
|
||||
{
|
||||
if(this->frameId().compare(imus[i].second->header.frame_id) != 0)
|
||||
{
|
||||
// We should not have to wait for IMU TF (imu delay <<< sensor data delay), so don't
|
||||
rtabmap::Transform localTransform = rtabmap_conversions::getTransform(this->frameId(), imus[i].second->header.frame_id, imus[i].second->header.stamp, *tfBuffer_, 0);
|
||||
if(localTransform.isNull())
|
||||
{
|
||||
if(imuLocalTransform_.isNull()) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Could not transform IMU msg from frame \"%s\" to frame \"%s\", TF is not available at IMU msg time %f. All IMU msgs up to sensor data time %f are skipped! If IMU TF is not static, make sure to publish it before the the imu topic is published.",
|
||||
imus[i].second->header.frame_id.c_str(), this->frameId().c_str(), rtabmap_conversions::timestampFromROS(imus[i].second->header.stamp), data.stamp());
|
||||
break;
|
||||
} else if(!imuWarnShown) {
|
||||
imuWarnShown = true; // show only one time
|
||||
RCLCPP_WARN(this->get_logger(), "Could not transform IMU msg from frame \"%s\" to frame \"%s\", TF is not available at IMU msg time %f. We will use latest known IMU local transform (if TF between camera/lidar and the IMU is static, you can safely ignore this warning and set always_check_imu_tf to false).",
|
||||
imus[i].second->header.frame_id.c_str(), this->frameId().c_str(), rtabmap_conversions::timestampFromROS(imus[i].second->header.stamp));
|
||||
}
|
||||
}
|
||||
else {
|
||||
imuLocalTransform_ = localTransform;
|
||||
}
|
||||
}
|
||||
else if(imuLocalTransform_.isNull())
|
||||
{
|
||||
imuLocalTransform_.setIdentity();
|
||||
}
|
||||
}
|
||||
|
||||
IMU imu(cv::Vec4d(imus[i].second->orientation.x, imus[i].second->orientation.y, imus[i].second->orientation.z, imus[i].second->orientation.w),
|
||||
cv::Mat(3,3,CV_64FC1,(void*)imus[i].second->orientation_covariance.data()).clone(),
|
||||
cv::Vec3d(imus[i].second->angular_velocity.x, imus[i].second->angular_velocity.y, imus[i].second->angular_velocity.z),
|
||||
cv::Mat(3,3,CV_64FC1,(void*)imus[i].second->angular_velocity_covariance.data()).clone(),
|
||||
cv::Vec3d(imus[i].second->linear_acceleration.x, imus[i].second->linear_acceleration.y, imus[i].second->linear_acceleration.z),
|
||||
cv::Mat(3,3,CV_64FC1,(void*)imus[i].second->linear_acceleration_covariance.data()).clone(),
|
||||
imuLocalTransform_);
|
||||
|
||||
SensorData dataIMU(imu, 0, imus[i].first);
|
||||
odometry_->process(dataIMU);
|
||||
imus_.erase(iter++);
|
||||
imuProcessed_ = true;
|
||||
}
|
||||
|
||||
//RCLCPP_WARN(get_logger(), "img callback: process image %f", timestampFromROS(header.stamp));
|
||||
|
||||
Transform groundTruth;
|
||||
if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
|
||||
{
|
||||
@@ -607,7 +677,7 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
|
||||
bool tooOldPreviousData = minUpdateRate_ > 0 && previousStamp_ > 0 && (rtabmap_conversions::timestampFromROS(header.stamp)-previousStamp_) > 1.0/minUpdateRate_;
|
||||
|
||||
// process data
|
||||
rclcpp::Time timeStart = now();
|
||||
rclcpp::Time timeStart = rclcpp::Clock().now();
|
||||
rtabmap::OdometryInfo info;
|
||||
if(!groundTruth.isNull())
|
||||
{
|
||||
@@ -929,124 +999,124 @@ void OdometryROS::processData(SensorData & data, const std_msgs::msg::Header & h
|
||||
}
|
||||
}
|
||||
|
||||
if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
|
||||
if(odomSensorDataPub_->get_subscription_count()>0 || odomSensorDataFeaturesPub_->get_subscription_count()>0)
|
||||
{
|
||||
if(odomSensorDataPub_->get_subscription_count()>0 || odomSensorDataFeaturesPub_->get_subscription_count()>0)
|
||||
rtabmap_msgs::msg::SensorData msg;
|
||||
rtabmap_conversions::sensorDataToROS(data, msg, frameId_, odomSensorDataPub_->get_subscription_count()>0);
|
||||
msg.header.stamp = header.stamp; // use corresponding time stamp to image
|
||||
if(odomSensorDataPub_->get_subscription_count()>0)
|
||||
{
|
||||
rtabmap_msgs::msg::SensorData msg;
|
||||
rtabmap_conversions::sensorDataToROS(data, msg, frameId_, odomSensorDataPub_->get_subscription_count()>0);
|
||||
msg.header.stamp = header.stamp; // use corresponding time stamp to image
|
||||
if(odomSensorDataPub_->get_subscription_count()>0)
|
||||
{
|
||||
odomSensorDataPub_->publish(msg);
|
||||
}
|
||||
if(odomSensorDataFeaturesPub_->get_subscription_count()>0)
|
||||
{
|
||||
// remove data
|
||||
msg.left = sensor_msgs::msg::Image();
|
||||
msg.right = sensor_msgs::msg::Image();
|
||||
msg.laser_scan = sensor_msgs::msg::PointCloud2();
|
||||
msg.grid_ground.clear();
|
||||
msg.grid_obstacles.clear();
|
||||
msg.grid_empty_cells.clear();
|
||||
odomSensorDataFeaturesPub_->publish(msg);
|
||||
}
|
||||
odomSensorDataPub_->publish(msg);
|
||||
}
|
||||
if(odomSensorDataCompressedPub_->get_subscription_count()>0)
|
||||
if(odomSensorDataFeaturesPub_->get_subscription_count()>0)
|
||||
{
|
||||
cv::Mat compressedImage;
|
||||
cv::Mat compressedDepth;
|
||||
cv::Mat compressedScan;
|
||||
if(compressionParallelized_)
|
||||
{
|
||||
rtabmap::CompressionThread ctImage(data.imageRaw(), compressionImgFormat_);
|
||||
rtabmap::CompressionThread ctDepth(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_);
|
||||
rtabmap::CompressionThread ctLaserScan(data.laserScanRaw().data());
|
||||
if(!data.imageRaw().empty())
|
||||
{
|
||||
ctImage.start();
|
||||
}
|
||||
if(!data.depthOrRightRaw().empty())
|
||||
{
|
||||
ctDepth.start();
|
||||
}
|
||||
if(!data.laserScanRaw().isEmpty())
|
||||
{
|
||||
ctLaserScan.start();
|
||||
}
|
||||
ctImage.join();
|
||||
ctDepth.join();
|
||||
ctLaserScan.join();
|
||||
|
||||
compressedImage = ctImage.getCompressedData();
|
||||
compressedDepth = ctDepth.getCompressedData();
|
||||
compressedScan = ctLaserScan.getCompressedData();
|
||||
}
|
||||
else
|
||||
{
|
||||
compressedImage = compressImage2(data.imageRaw(), compressionImgFormat_);
|
||||
compressedDepth = compressImage2(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_);
|
||||
compressedScan = compressData2(data.laserScanRaw().data());
|
||||
}
|
||||
if(!compressedImage.empty() && !data.stereoCameraModels().empty())
|
||||
{
|
||||
data.setStereoImage(compressedImage, compressedDepth, data.stereoCameraModels(), false);
|
||||
}
|
||||
else if(!compressedImage.empty() && !data.cameraModels().empty())
|
||||
{
|
||||
data.setRGBDImage(compressedImage, compressedDepth, data.cameraModels(), false);
|
||||
}
|
||||
if(!compressedScan.empty())
|
||||
{
|
||||
data.setLaserScan(data.laserScanRaw().angleIncrement() == 0.0f?
|
||||
LaserScan(compressedScan,
|
||||
data.laserScanRaw().maxPoints(),
|
||||
data.laserScanRaw().rangeMax(),
|
||||
data.laserScanRaw().format(),
|
||||
data.laserScanRaw().localTransform()):
|
||||
LaserScan(compressedScan,
|
||||
data.laserScanRaw().format(),
|
||||
data.laserScanRaw().rangeMin(),
|
||||
data.laserScanRaw().rangeMax(),
|
||||
data.laserScanRaw().angleMin(),
|
||||
data.laserScanRaw().angleMax(),
|
||||
data.laserScanRaw().angleIncrement(),
|
||||
data.laserScanRaw().localTransform()), false);
|
||||
}
|
||||
rtabmap_msgs::msg::SensorData msg;
|
||||
rtabmap_conversions::sensorDataToROS(data, msg, frameId_, false);
|
||||
msg.header.stamp = header.stamp; // use corresponding time stamp to image
|
||||
odomSensorDataCompressedPub_->publish(msg);
|
||||
// remove data
|
||||
msg.left = sensor_msgs::msg::Image();
|
||||
msg.right = sensor_msgs::msg::Image();
|
||||
msg.laser_scan = sensor_msgs::msg::PointCloud2();
|
||||
msg.grid_ground.clear();
|
||||
msg.grid_obstacles.clear();
|
||||
msg.grid_empty_cells.clear();
|
||||
odomSensorDataFeaturesPub_->publish(msg);
|
||||
}
|
||||
|
||||
if(visParams_)
|
||||
{
|
||||
if(icpParams_)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (now()-timeStart).seconds());
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (now()-timeStart).seconds());
|
||||
}
|
||||
}
|
||||
else // if(icpParams_)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (now()-timeStart).seconds());
|
||||
}
|
||||
|
||||
statusDiagnostic_.setStatus(pose.isNull());
|
||||
if(syncDiagnostic_.get() && !pose.isNull())
|
||||
{
|
||||
double curentRate = 1.0/(this->now()-timeStart).seconds();
|
||||
syncDiagnostic_->tick(header.stamp,
|
||||
maxUpdateRate_>0 ? maxUpdateRate_:
|
||||
expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_:
|
||||
previousStamp_ == 0.0 || rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_ > 1.0/curentRate?0:curentRate);
|
||||
}
|
||||
|
||||
previousStamp_ = rtabmap_conversions::timestampFromROS(header.stamp);
|
||||
}
|
||||
if(odomSensorDataCompressedPub_->get_subscription_count()>0)
|
||||
{
|
||||
cv::Mat compressedImage;
|
||||
cv::Mat compressedDepth;
|
||||
cv::Mat compressedScan;
|
||||
if(compressionParallelized_)
|
||||
{
|
||||
rtabmap::CompressionThread ctImage(data.imageRaw(), compressionImgFormat_);
|
||||
rtabmap::CompressionThread ctDepth(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_);
|
||||
rtabmap::CompressionThread ctLaserScan(data.laserScanRaw().data());
|
||||
if(!data.imageRaw().empty())
|
||||
{
|
||||
ctImage.start();
|
||||
}
|
||||
if(!data.depthOrRightRaw().empty())
|
||||
{
|
||||
ctDepth.start();
|
||||
}
|
||||
if(!data.laserScanRaw().isEmpty())
|
||||
{
|
||||
ctLaserScan.start();
|
||||
}
|
||||
ctImage.join();
|
||||
ctDepth.join();
|
||||
ctLaserScan.join();
|
||||
|
||||
compressedImage = ctImage.getCompressedData();
|
||||
compressedDepth = ctDepth.getCompressedData();
|
||||
compressedScan = ctLaserScan.getCompressedData();
|
||||
}
|
||||
else
|
||||
{
|
||||
compressedImage = compressImage2(data.imageRaw(), compressionImgFormat_);
|
||||
compressedDepth = compressImage2(data.depthOrRightRaw(), data.depthOrRightRaw().type() == CV_32FC1 || data.depthOrRightRaw().type() == CV_16UC1?std::string(".png"):compressionImgFormat_);
|
||||
compressedScan = compressData2(data.laserScanRaw().data());
|
||||
}
|
||||
if(!compressedImage.empty() && !data.stereoCameraModels().empty())
|
||||
{
|
||||
data.setStereoImage(compressedImage, compressedDepth, data.stereoCameraModels(), false);
|
||||
}
|
||||
else if(!compressedImage.empty() && !data.cameraModels().empty())
|
||||
{
|
||||
data.setRGBDImage(compressedImage, compressedDepth, data.cameraModels(), false);
|
||||
}
|
||||
if(!compressedScan.empty())
|
||||
{
|
||||
data.setLaserScan(data.laserScanRaw().angleIncrement() == 0.0f?
|
||||
LaserScan(compressedScan,
|
||||
data.laserScanRaw().maxPoints(),
|
||||
data.laserScanRaw().rangeMax(),
|
||||
data.laserScanRaw().format(),
|
||||
data.laserScanRaw().localTransform()):
|
||||
LaserScan(compressedScan,
|
||||
data.laserScanRaw().format(),
|
||||
data.laserScanRaw().rangeMin(),
|
||||
data.laserScanRaw().rangeMax(),
|
||||
data.laserScanRaw().angleMin(),
|
||||
data.laserScanRaw().angleMax(),
|
||||
data.laserScanRaw().angleIncrement(),
|
||||
data.laserScanRaw().localTransform()), false);
|
||||
}
|
||||
rtabmap_msgs::msg::SensorData msg;
|
||||
rtabmap_conversions::sensorDataToROS(data, msg, frameId_, false);
|
||||
msg.header.stamp = header.stamp; // use corresponding time stamp to image
|
||||
odomSensorDataCompressedPub_->publish(msg);
|
||||
}
|
||||
|
||||
double delay = (now()-header.stamp).seconds();
|
||||
if(visParams_)
|
||||
{
|
||||
if(icpParams_)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (rclcpp::Clock().now()-timeStart).seconds(), delay);
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Odom: quality=%d, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (rclcpp::Clock().now()-timeStart).seconds(), delay);
|
||||
}
|
||||
}
|
||||
else // if(icpParams_)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs delay=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (rclcpp::Clock().now()-timeStart).seconds(), delay);
|
||||
}
|
||||
|
||||
statusDiagnostic_.setStatus(pose.isNull(), processedMsgs_, droppedMsgs_);
|
||||
processedMsgs_ = 0;
|
||||
droppedMsgs_ = 0;
|
||||
if(syncDiagnostic_.get())
|
||||
{
|
||||
double curentRate = 1.0/(rclcpp::Clock().now()-timeStart).seconds();
|
||||
syncDiagnostic_->tickOutput(header.stamp,
|
||||
maxUpdateRate_>0 ? maxUpdateRate_:
|
||||
expectedUpdateRate_>0 && expectedUpdateRate_ < curentRate ? expectedUpdateRate_:
|
||||
previousStamp_ == 0.0 || rtabmap_conversions::timestampFromROS(header.stamp) - previousStamp_ > 1.0/curentRate?0:curentRate);
|
||||
}
|
||||
|
||||
previousStamp_ = rtabmap_conversions::timestampFromROS(header.stamp);
|
||||
}
|
||||
|
||||
void OdometryROS::resetOdom(
|
||||
@@ -1070,14 +1140,19 @@ void OdometryROS::resetToPose(
|
||||
|
||||
void OdometryROS::reset(const Transform & pose)
|
||||
{
|
||||
UScopeMutex lock(dataMutex_);
|
||||
odometry_->reset(pose);
|
||||
guess_.setNull();
|
||||
guessPreviousPose_.setNull();
|
||||
previousStamp_ = 0.0;
|
||||
resetCurrentCount_ = resetCountdown_;
|
||||
imuProcessed_ = false;
|
||||
bufferedData_.first= SensorData();
|
||||
dataToProcess_ = SensorData();
|
||||
dataHeaderToProcess_ = std_msgs::msg::Header();
|
||||
imuMutex_.lock();
|
||||
imus_.clear();
|
||||
imuMutex_.unlock();
|
||||
imuLocalTransform_.setNull();
|
||||
this->flushCallbacks();
|
||||
}
|
||||
|
||||
@@ -1149,13 +1224,17 @@ void OdometryROS::setLogError(
|
||||
OdometryROS::OdomStatusTask::OdomStatusTask() :
|
||||
diagnostic_updater::DiagnosticTask("Odom status"),
|
||||
lost_(false),
|
||||
dataReceived_(false)
|
||||
dataReceived_(false),
|
||||
processedMsgs_(0),
|
||||
droppedMsgs_(0)
|
||||
{}
|
||||
|
||||
void OdometryROS::OdomStatusTask::setStatus(bool isLost)
|
||||
void OdometryROS::OdomStatusTask::setStatus(bool isLost, int processedMsgs, int droppedMsgs)
|
||||
{
|
||||
dataReceived_ = true;
|
||||
lost_ = isLost;
|
||||
processedMsgs_ += processedMsgs;
|
||||
droppedMsgs_ += droppedMsgs;
|
||||
}
|
||||
|
||||
void OdometryROS::OdomStatusTask::run(diagnostic_updater::DiagnosticStatusWrapper &stat)
|
||||
@@ -1172,6 +1251,18 @@ void OdometryROS::OdomStatusTask::run(diagnostic_updater::DiagnosticStatusWrappe
|
||||
{
|
||||
stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Tracking.");
|
||||
}
|
||||
stat.add("Topics Processed", processedMsgs_);
|
||||
stat.add("Topics Dropped", droppedMsgs_);
|
||||
processedMsgs_ = 0;
|
||||
droppedMsgs_ = 0;
|
||||
}
|
||||
|
||||
void OdometryROS::tick(const rclcpp::Time & stamp)
|
||||
{
|
||||
if(syncDiagnostic_.get())
|
||||
{
|
||||
syncDiagnostic_->tickInput(stamp);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -76,7 +76,10 @@ int main(int argc, char **argv)
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::NodeOptions options;
|
||||
options.arguments(arguments);
|
||||
rclcpp::spin(std::make_shared<rtabmap_odom::RGBDOdometry>(options));
|
||||
auto node = std::make_shared<rtabmap_odom::RGBDOdometry>(options);
|
||||
rclcpp::executors::MultiThreadedExecutor executor;
|
||||
executor.add_node(node);
|
||||
executor.spin();
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -76,7 +76,10 @@ int main(int argc, char **argv)
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::NodeOptions options;
|
||||
options.arguments(arguments);
|
||||
rclcpp::spin(std::make_shared<rtabmap_odom::StereoOdometry>(options));
|
||||
auto node = std::make_shared<rtabmap_odom::StereoOdometry>(options);
|
||||
rclcpp::executors::MultiThreadedExecutor executor;
|
||||
executor.add_node(node);
|
||||
executor.spin();
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -97,8 +97,11 @@ void ICPOdometry::onOdomInit()
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing = %s", deskewing_?"true":"false");
|
||||
RCLCPP_INFO(this->get_logger(), "IcpOdometry: deskewing_slerp = %s", deskewingSlerp_?"true":"false");
|
||||
|
||||
scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1));
|
||||
cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1));
|
||||
rclcpp::SubscriptionOptions options;
|
||||
options.callback_group = dataCallbackGroup_;
|
||||
|
||||
scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackScan, this, std::placeholders::_1), options);
|
||||
cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&ICPOdometry::callbackCloud, this, std::placeholders::_1), options);
|
||||
|
||||
filtered_scan_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>("odom_filtered_input_scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()));
|
||||
|
||||
@@ -277,6 +280,9 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
|
||||
scan_sub_.reset();
|
||||
return;
|
||||
}
|
||||
|
||||
tick(scanMsg->header.stamp);
|
||||
|
||||
scanReceived_ = true;
|
||||
if(this->isPaused())
|
||||
{
|
||||
@@ -342,6 +348,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
|
||||
|
||||
sensor_msgs::msg::PointCloud2 scanOutDeskewed;
|
||||
rtabmap_conversions::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed);
|
||||
scanOutDeskewed.header.frame_id = scanMsg->header.frame_id;
|
||||
scanOut = scanOutDeskewed;
|
||||
}
|
||||
else
|
||||
@@ -520,6 +527,9 @@ void ICPOdometry::callbackCloud(const sensor_msgs::msg::PointCloud2::SharedPtr p
|
||||
cloud_sub_.reset();
|
||||
return;
|
||||
}
|
||||
|
||||
tick(pointCloudMsg->header.stamp);
|
||||
|
||||
cloudReceived_ = true;
|
||||
if(this->isPaused())
|
||||
{
|
||||
|
||||
@@ -63,7 +63,7 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) :
|
||||
exactSync5_(0),
|
||||
approxSync6_(0),
|
||||
exactSync6_(0),
|
||||
topicQueueSize_(1),
|
||||
topicQueueSize_(10),
|
||||
syncQueueSize_(5),
|
||||
keepColor_(false)
|
||||
{
|
||||
@@ -108,9 +108,9 @@ void RGBDOdometry::onOdomInit()
|
||||
int qosCamInfo = this->declare_parameter("qos_camera_info", (int)qos());
|
||||
subscribeRGBD = this->declare_parameter("subscribe_rgbd", subscribeRGBD);
|
||||
rgbdCameras = this->declare_parameter("rgbd_cameras", rgbdCameras);
|
||||
if(rgbdCameras <= 0)
|
||||
if(rgbdCameras < 0)
|
||||
{
|
||||
rgbdCameras = 1;
|
||||
rgbdCameras = 0;
|
||||
}
|
||||
keepColor_ = this->declare_parameter("keep_color", keepColor_);
|
||||
|
||||
@@ -125,29 +125,32 @@ void RGBDOdometry::onOdomInit()
|
||||
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: rgbd_cameras = %d", rgbdCameras);
|
||||
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: keep_color = %s", keepColor_?"true":"false");
|
||||
|
||||
rclcpp::SubscriptionOptions options;
|
||||
options.callback_group = dataCallbackGroup_;
|
||||
|
||||
std::string subscribedTopic;
|
||||
std::string subscribedTopicsMsg;
|
||||
if(subscribeRGBD)
|
||||
{
|
||||
if(rgbdCameras >= 2)
|
||||
{
|
||||
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
if(rgbdCameras >= 3)
|
||||
{
|
||||
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
}
|
||||
if(rgbdCameras >= 4)
|
||||
{
|
||||
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
}
|
||||
if(rgbdCameras >= 5)
|
||||
{
|
||||
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
}
|
||||
if(rgbdCameras >= 6)
|
||||
{
|
||||
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
}
|
||||
|
||||
if(rgbdCameras == 2)
|
||||
@@ -327,7 +330,7 @@ void RGBDOdometry::onOdomInit()
|
||||
}
|
||||
else if(rgbdCameras == 0)
|
||||
{
|
||||
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1));
|
||||
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBDX, this, std::placeholders::_1), options);
|
||||
|
||||
subscribedTopic = rgbdxSub_->get_topic_name();
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to:\n %s",
|
||||
@@ -336,7 +339,7 @@ void RGBDOdometry::onOdomInit()
|
||||
}
|
||||
else
|
||||
{
|
||||
rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1));
|
||||
rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&RGBDOdometry::callbackRGBD, this, std::placeholders::_1), options);
|
||||
|
||||
subscribedTopic = rgbdSub_->get_topic_name();
|
||||
subscribedTopicsMsg =
|
||||
@@ -348,9 +351,9 @@ void RGBDOdometry::onOdomInit()
|
||||
else
|
||||
{
|
||||
image_transport::TransportHints hints(this);
|
||||
image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
|
||||
image_mono_sub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
image_depth_sub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
info_sub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile(), options);
|
||||
|
||||
if(approxSync)
|
||||
{
|
||||
@@ -366,10 +369,12 @@ void RGBDOdometry::onOdomInit()
|
||||
}
|
||||
|
||||
subscribedTopic = image_mono_sub_.getSubscriber().getTopic();
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s",
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s, topic_queue_size=%d, sync_queue_size=%d):\n %s,\n %s,\n %s",
|
||||
get_name(),
|
||||
approxSync?"approx":"exact",
|
||||
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
|
||||
topicQueueSize_,
|
||||
syncQueueSize_,
|
||||
image_mono_sub_.getSubscriber().getTopic().c_str(),
|
||||
image_depth_sub_.getSubscriber().getTopic().c_str(),
|
||||
info_sub_.getSubscriber()->get_topic_name());
|
||||
@@ -564,6 +569,8 @@ void RGBDOdometry::callback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depth,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
|
||||
{
|
||||
tick(image->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
|
||||
@@ -592,6 +599,8 @@ void RGBDOdometry::callback(
|
||||
void RGBDOdometry::callbackRGBDX(
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images)
|
||||
{
|
||||
tick(images->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
if(images->rgbd_images.empty())
|
||||
@@ -615,6 +624,8 @@ void RGBDOdometry::callbackRGBDX(
|
||||
void RGBDOdometry::callbackRGBD(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image)
|
||||
{
|
||||
tick(image->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
|
||||
@@ -631,6 +642,8 @@ void RGBDOdometry::callbackRGBD2(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2)
|
||||
{
|
||||
tick(image->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2);
|
||||
@@ -650,6 +663,8 @@ void RGBDOdometry::callbackRGBD3(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3)
|
||||
{
|
||||
tick(image->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(3);
|
||||
@@ -672,6 +687,8 @@ void RGBDOdometry::callbackRGBD4(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4)
|
||||
{
|
||||
tick(image->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(4);
|
||||
@@ -697,6 +714,8 @@ void RGBDOdometry::callbackRGBD5(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5)
|
||||
{
|
||||
tick(image->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(5);
|
||||
@@ -725,6 +744,8 @@ void RGBDOdometry::callbackRGBD6(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6)
|
||||
{
|
||||
tick(image->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(6);
|
||||
|
||||
@@ -63,7 +63,7 @@ StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) :
|
||||
exactSync5_(0),
|
||||
approxSync6_(0),
|
||||
exactSync6_(0),
|
||||
topicQueueSize_(1),
|
||||
topicQueueSize_(10),
|
||||
syncQueueSize_(5),
|
||||
keepColor_(false)
|
||||
{
|
||||
@@ -120,29 +120,32 @@ void StereoOdometry::onOdomInit()
|
||||
RCLCPP_INFO(this->get_logger(), "StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
|
||||
RCLCPP_INFO(this->get_logger(), "StereoOdometry: keep_color = %s", keepColor_?"true":"false");
|
||||
|
||||
rclcpp::SubscriptionOptions options;
|
||||
options.callback_group = dataCallbackGroup_;
|
||||
|
||||
std::string subscribedTopic;
|
||||
std::string subscribedTopicsMsg;
|
||||
if(subscribeRGBD)
|
||||
{
|
||||
if(rgbdCameras >= 2)
|
||||
{
|
||||
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image1_sub_.subscribe(this, "rgbd_image0", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
rgbd_image2_sub_.subscribe(this, "rgbd_image1", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
if(rgbdCameras >= 3)
|
||||
{
|
||||
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image3_sub_.subscribe(this, "rgbd_image2", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
}
|
||||
if(rgbdCameras >= 4)
|
||||
{
|
||||
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
}
|
||||
if(rgbdCameras >= 5)
|
||||
{
|
||||
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
}
|
||||
if(rgbdCameras >= 6)
|
||||
{
|
||||
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
}
|
||||
|
||||
if(rgbdCameras == 2)
|
||||
@@ -323,7 +326,7 @@ void StereoOdometry::onOdomInit()
|
||||
}
|
||||
else if(rgbdCameras == 0)
|
||||
{
|
||||
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBDX, this, std::placeholders::_1));
|
||||
rgbdxSub_ = create_subscription<rtabmap_msgs::msg::RGBDImages>("rgbd_images", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBDX, this, std::placeholders::_1), options);
|
||||
|
||||
subscribedTopic = rgbdxSub_->get_topic_name();
|
||||
subscribedTopicsMsg =
|
||||
@@ -333,7 +336,7 @@ void StereoOdometry::onOdomInit()
|
||||
}
|
||||
else
|
||||
{
|
||||
rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1));
|
||||
rgbdSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()), std::bind(&StereoOdometry::callbackRGBD, this, std::placeholders::_1), options);
|
||||
|
||||
subscribedTopic = rgbdSub_->get_topic_name();
|
||||
subscribedTopicsMsg =
|
||||
@@ -345,10 +348,10 @@ void StereoOdometry::onOdomInit()
|
||||
else
|
||||
{
|
||||
image_transport::TransportHints hints(this);
|
||||
imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
|
||||
cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
|
||||
cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
|
||||
imageRectLeft_.subscribe(this, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
imageRectRight_.subscribe(this, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile(), options);
|
||||
cameraInfoLeft_.subscribe(this, "left/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile(), options);
|
||||
cameraInfoRight_.subscribe(this, "right/camera_info", rclcpp::QoS(topicQueueSize_).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile(), options);
|
||||
|
||||
if(approxSync)
|
||||
{
|
||||
@@ -364,10 +367,12 @@ void StereoOdometry::onOdomInit()
|
||||
}
|
||||
|
||||
subscribedTopic = imageRectLeft_.getTopic();
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
|
||||
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s, topic_queue_size=%d, sync_queue_size=%d):\n %s \\\n %s \\\n %s \\\n %s",
|
||||
get_name(),
|
||||
approxSync?"approx":"exact",
|
||||
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
|
||||
topicQueueSize_,
|
||||
syncQueueSize_,
|
||||
imageRectLeft_.getTopic().c_str(),
|
||||
imageRectRight_.getTopic().c_str(),
|
||||
cameraInfoLeft_.getSubscriber()->get_topic_name(),
|
||||
@@ -709,6 +714,8 @@ void StereoOdometry::callback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight)
|
||||
{
|
||||
tick(imageRectLeft->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
|
||||
@@ -739,6 +746,8 @@ void StereoOdometry::callback(
|
||||
void StereoOdometry::callbackRGBD(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image)
|
||||
{
|
||||
tick(image->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
|
||||
@@ -756,6 +765,8 @@ void StereoOdometry::callbackRGBD(
|
||||
void StereoOdometry::callbackRGBDX(
|
||||
const rtabmap_msgs::msg::RGBDImages::ConstSharedPtr images)
|
||||
{
|
||||
tick(images->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
if(images->rgbd_images.empty())
|
||||
@@ -782,6 +793,8 @@ void StereoOdometry::callbackRGBD2(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2)
|
||||
{
|
||||
tick(image->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(2);
|
||||
@@ -804,6 +817,8 @@ void StereoOdometry::callbackRGBD3(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3)
|
||||
{
|
||||
tick(image->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(3);
|
||||
@@ -830,6 +845,8 @@ void StereoOdometry::callbackRGBD4(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4)
|
||||
{
|
||||
tick(image->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(4);
|
||||
@@ -860,6 +877,8 @@ void StereoOdometry::callbackRGBD5(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5)
|
||||
{
|
||||
tick(image->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(5);
|
||||
@@ -894,6 +913,8 @@ void StereoOdometry::callbackRGBD6(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6)
|
||||
{
|
||||
tick(image->header.stamp);
|
||||
|
||||
if(!this->isPaused())
|
||||
{
|
||||
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(6);
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>rtabmap_python</name>
|
||||
<version>0.21.5</version>
|
||||
<version>0.21.9</version>
|
||||
<description>RTAB-Map's python package.</description>
|
||||
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
|
||||
<author>Mathieu Labbe</author>
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>rtabmap_ros</name>
|
||||
<version>0.21.5</version>
|
||||
<version>0.21.9</version>
|
||||
<description>
|
||||
RTAB-Map Stack
|
||||
</description>
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>rtabmap_rviz_plugins</name>
|
||||
<version>0.21.5</version>
|
||||
<version>0.21.9</version>
|
||||
<description>RTAB-Map's rviz plugins.</description>
|
||||
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
|
||||
<author>Mathieu Labbe</author>
|
||||
|
||||
@@ -319,7 +319,8 @@ void MapCloudDisplay::processMapData(const rtabmap_msgs::msg::MapData& map)
|
||||
cloud_filter_floor_height_->getFloat()!=0.0f?cloud_filter_floor_height_->getFloat():-999.0f,
|
||||
cloud_filter_ceiling_height_->getFloat()!=0.0f && (cloud_filter_floor_height_->getFloat()==0.0f || cloud_filter_ceiling_height_->getFloat()>cloud_filter_floor_height_->getFloat())?cloud_filter_ceiling_height_->getFloat():999.0f);
|
||||
// convert back in /base_link frame
|
||||
cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose().inverse());
|
||||
if(!cloud->empty())
|
||||
cloud = rtabmap::util3d::transformPointCloud(cloud, s.getPose().inverse());
|
||||
}
|
||||
|
||||
if(!cloud->empty())
|
||||
|
||||
@@ -118,8 +118,9 @@ public:
|
||||
|
||||
private:
|
||||
bool odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Time stamp);
|
||||
bool odomTFUpdate(const rclcpp::Time & stamp); // TF odom
|
||||
bool odomTFUpdate(const std::string & odomFrameId, const rclcpp::Time & stamp); // TF odom
|
||||
|
||||
// Callback called from sync thread
|
||||
virtual void commonMultiCameraCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
|
||||
@@ -134,6 +135,7 @@ private:
|
||||
const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPoints = std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> >(),
|
||||
const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3d = std::vector<std::vector<rtabmap_msgs::msg::Point3f> >(),
|
||||
const std::vector<cv::Mat> & localDescriptors = std::vector<cv::Mat>());
|
||||
// Callback called from sync thread
|
||||
void commonMultiCameraCallbackImpl(
|
||||
const std::string & odomFrameId,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
|
||||
@@ -148,6 +150,7 @@ private:
|
||||
const std::vector<std::vector<rtabmap_msgs::msg::KeyPoint> > & localKeyPoints,
|
||||
const std::vector<std::vector<rtabmap_msgs::msg::Point3f> > & localPoints3d,
|
||||
const std::vector<cv::Mat> & localDescriptors);
|
||||
// Callback called from sync thread
|
||||
virtual void commonLaserScanCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
|
||||
@@ -155,11 +158,13 @@ private:
|
||||
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
|
||||
const rtabmap_msgs::msg::GlobalDescriptor & globalDescriptor = rtabmap_msgs::msg::GlobalDescriptor());
|
||||
// Callback called from sync thread
|
||||
virtual void commonOdomCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg);
|
||||
|
||||
// Callback called from sync thread
|
||||
virtual void commonSensorDataCallback(
|
||||
const rtabmap_msgs::msg::SensorData::ConstSharedPtr & sensorDataMsg,
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
@@ -195,6 +200,8 @@ private:
|
||||
void goalNodeCallback(const rtabmap_msgs::msg::Goal::SharedPtr msg);
|
||||
void updateGoal(const rclcpp::Time & stamp);
|
||||
|
||||
void processAsync();
|
||||
|
||||
void process(
|
||||
const rclcpp::Time & stamp,
|
||||
rtabmap::SensorData & data,
|
||||
@@ -254,7 +261,7 @@ private:
|
||||
#ifdef NAV_MSGS_FOXY
|
||||
void goalResponseCallback(std::shared_future<GoalHandleNav2::SharedPtr> future);
|
||||
#else
|
||||
void goalResponseCallback(const GoalHandleNav2::SharedPtr & goal_handle);
|
||||
void goalResponseCallback(const GoalHandleNav2::SharedPtr & goal_handle);
|
||||
#endif
|
||||
void resultCallback(const GoalHandleNav2::WrappedResult & result);
|
||||
#endif
|
||||
@@ -266,11 +273,14 @@ private:
|
||||
private:
|
||||
rtabmap::Rtabmap rtabmap_;
|
||||
bool paused_;
|
||||
|
||||
UMutex lastPoseMutex_;
|
||||
rtabmap::Transform lastPose_;
|
||||
rclcpp::Time lastPoseStamp_;
|
||||
std::vector<float> lastPoseVelocity_;
|
||||
cv::Mat lastPoseCovariance_;
|
||||
bool lastPoseIntermediate_;
|
||||
cv::Mat covariance_;
|
||||
|
||||
rtabmap::Transform currentMetricGoal_;
|
||||
rtabmap::Transform lastPublishedMetricGoal_;
|
||||
bool latestNodeWasReached_;
|
||||
@@ -341,7 +351,7 @@ private:
|
||||
std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tfListener_;
|
||||
|
||||
rclcpp::SyncParametersClient::SharedPtr parametersClient_;
|
||||
rclcpp::AsyncParametersClient::SharedPtr parametersClient_;
|
||||
rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameterEventSub_;
|
||||
|
||||
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr updateSrv_;
|
||||
@@ -381,6 +391,7 @@ private:
|
||||
#endif
|
||||
#ifdef WITH_NAV2_MSGS
|
||||
rclcpp_action::Client<NavigateToPose>::SharedPtr nav2Client_;
|
||||
rclcpp_action::GoalUUID lastGoalSent_;
|
||||
#endif
|
||||
|
||||
std::thread* transformThread_;
|
||||
@@ -389,6 +400,7 @@ private:
|
||||
// for loop closure detection only
|
||||
image_transport::Subscriber defaultSub_;
|
||||
|
||||
rclcpp::CallbackGroup::SharedPtr userDataAsyncCallbackGroup_;
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::UserData>::SharedPtr userDataAsyncSub_;
|
||||
cv::Mat userData_;
|
||||
UMutex userDataMutex_;
|
||||
@@ -397,6 +409,8 @@ private:
|
||||
geometry_msgs::msg::PoseWithCovarianceStamped globalPose_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gpsFixAsyncSub_;
|
||||
rtabmap::GPS gps_;
|
||||
|
||||
rclcpp::CallbackGroup::SharedPtr landmarkCallbackGroup_;
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::LandmarkDetection>::SharedPtr landmarkDetectionSub_;
|
||||
rclcpp::Subscription<rtabmap_msgs::msg::LandmarkDetections>::SharedPtr landmarkDetectionsSub_;
|
||||
#ifdef WITH_APRILTAG_MSGS
|
||||
@@ -406,10 +420,14 @@ private:
|
||||
rclcpp::Subscription<fiducial_msgs::msg::FiducialTransformArray>::SharedPtr fiducialTransfromsSub_;
|
||||
#endif
|
||||
std::map<int, std::pair<geometry_msgs::msg::PoseWithCovarianceStamped, float> > landmarks_; // id, <pose, size>
|
||||
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imuSub_;
|
||||
UMutex landmarksMutex_;
|
||||
|
||||
rclcpp::CallbackGroup::SharedPtr imuCallbackGroup_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imuSub_;
|
||||
std::map<double, rtabmap::Transform> imus_;
|
||||
std::string imuFrameId_;
|
||||
UMutex imuMutex_;
|
||||
|
||||
rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr republishNodeDataSub_;
|
||||
|
||||
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr interOdomSub_;
|
||||
@@ -443,6 +461,23 @@ private:
|
||||
double localizationError_;
|
||||
};
|
||||
LocalizationStatusTask localizationDiagnostic_;
|
||||
|
||||
rclcpp::CallbackGroup::SharedPtr processingCallbackGroup_;
|
||||
struct SyncData {
|
||||
bool valid;
|
||||
rclcpp::Time stamp;
|
||||
rtabmap::SensorData data;
|
||||
rtabmap::Transform odom;
|
||||
std::vector<float> odomVelocity;
|
||||
std::string odomFrameId;
|
||||
cv::Mat odomCovariance;
|
||||
rtabmap::OdometryInfo odomInfo;
|
||||
double timeMsgConversion;
|
||||
};
|
||||
rclcpp::TimerBase::SharedPtr syncTimer_;
|
||||
SyncData syncData_;
|
||||
UMutex syncDataMutex_;
|
||||
bool triggerNewMapBeforeNextUpdate_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>rtabmap_slam</name>
|
||||
<version>0.21.5</version>
|
||||
<version>0.21.9</version>
|
||||
<description>RTAB-Map's SLAM package.</description>
|
||||
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
|
||||
<author>Mathieu Labbe</author>
|
||||
|
||||
@@ -84,8 +84,11 @@ int main(int argc, char** argv)
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::NodeOptions options;
|
||||
options.arguments(arguments);
|
||||
auto node = std::make_shared<rtabmap_slam::CoreWrapper>(options);
|
||||
rclcpp::executors::MultiThreadedExecutor executor;
|
||||
executor.add_node(node);
|
||||
UINFO("rtabmap %s started...", RTABMAP_VERSION);
|
||||
rclcpp::spin(std::make_shared<rtabmap_slam::CoreWrapper>(options));
|
||||
executor.spin();
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
|
||||
+454
-287
File diff suppressed because it is too large
Load Diff
@@ -117,26 +117,27 @@ protected:
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg) = 0;
|
||||
|
||||
void commonSingleCameraCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
|
||||
const cv_bridge::CvImageConstPtr & imageMsg,
|
||||
const cv_bridge::CvImageConstPtr & depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo & rgbCameraInfoMsg,
|
||||
const sensor_msgs::msg::CameraInfo & depthCameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan & scanMsg,
|
||||
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
|
||||
const std::vector<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_msgs::msg::GlobalDescriptor>(),
|
||||
const std::vector<rtabmap_msgs::msg::KeyPoint> & localKeyPoints = std::vector<rtabmap_msgs::msg::KeyPoint>(),
|
||||
const std::vector<rtabmap_msgs::msg::Point3f> & localPoints3d = std::vector<rtabmap_msgs::msg::Point3f>(),
|
||||
const cv::Mat & localDescriptors = cv::Mat());
|
||||
|
||||
void tick(const rclcpp::Time & stamp, double targetFrequency = 0);
|
||||
|
||||
private:
|
||||
void commonSingleCameraCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
|
||||
const cv_bridge::CvImageConstPtr & imageMsg,
|
||||
const cv_bridge::CvImageConstPtr & depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo & rgbCameraInfoMsg,
|
||||
const sensor_msgs::msg::CameraInfo & depthCameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan & scanMsg,
|
||||
const sensor_msgs::msg::PointCloud2 & scan3dMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg,
|
||||
const std::vector<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_msgs::msg::GlobalDescriptor>(),
|
||||
const std::vector<rtabmap_msgs::msg::KeyPoint> & localKeyPoints = std::vector<rtabmap_msgs::msg::KeyPoint>(),
|
||||
const std::vector<rtabmap_msgs::msg::Point3f> & localPoints3d = std::vector<rtabmap_msgs::msg::Point3f>(),
|
||||
const cv::Mat & localDescriptors = cv::Mat());
|
||||
void processSyncData();
|
||||
void setupDepthCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
@@ -145,10 +146,12 @@ private:
|
||||
bool subscribeOdomInfo);
|
||||
void setupStereoCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
@@ -157,6 +160,7 @@ private:
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBDCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
@@ -165,6 +169,7 @@ private:
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBDXCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
@@ -174,6 +179,7 @@ private:
|
||||
#ifdef RTABMAP_SYNC_MULTI_RGBD
|
||||
void setupRGBD2Callbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
@@ -182,6 +188,7 @@ private:
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBD3Callbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
@@ -190,6 +197,7 @@ private:
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBD4Callbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
@@ -198,6 +206,7 @@ private:
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBD5Callbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
@@ -206,6 +215,7 @@ private:
|
||||
bool subscribeOdomInfo);
|
||||
void setupRGBD6Callbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeUserData,
|
||||
bool subscribeScan2d,
|
||||
@@ -215,10 +225,12 @@ private:
|
||||
#endif
|
||||
void setupSensorDataCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
bool subscribeOdomInfo);
|
||||
void setupScanCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeScan2d,
|
||||
bool subscribeScanDesc,
|
||||
bool subscribeOdom,
|
||||
@@ -226,6 +238,7 @@ private:
|
||||
bool subscribeOdomInfo);
|
||||
void setupOdomCallbacks(
|
||||
rclcpp::Node & node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeUserData,
|
||||
bool subscribeOdomInfo);
|
||||
|
||||
@@ -257,6 +270,8 @@ private:
|
||||
int rgbdCameras_;
|
||||
std::string name_;
|
||||
|
||||
rclcpp::CallbackGroup::SharedPtr syncCallbackGroup_;
|
||||
|
||||
//for depth and rgb-only callbacks
|
||||
image_transport::SubscriberFilter imageSub_;
|
||||
image_transport::SubscriberFilter imageDepthSub_;
|
||||
|
||||
@@ -8,6 +8,7 @@
|
||||
|
||||
#include "rtabmap_conversions/MsgConversion.h"
|
||||
#include "rtabmap/utilite/ULogger.h"
|
||||
#include "rtabmap/utilite/UMutex.h"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
@@ -15,14 +16,19 @@ namespace rtabmap_sync {
|
||||
|
||||
class SyncDiagnostic {
|
||||
public:
|
||||
SyncDiagnostic(rclcpp::Node * node, double tolerance = 0.1, int windowSize = 5) :
|
||||
SyncDiagnostic(rclcpp::Node * node, double tolerance = 0.2, int windowSize = 5) :
|
||||
node_(node),
|
||||
diagnosticUpdater_(node),
|
||||
frequencyStatus_(diagnostic_updater::FrequencyStatusParam(&targetFrequency_, &targetFrequency_, tolerance), node->get_clock()),
|
||||
timeStampStatus_(diagnostic_updater::TimeStampStatusParam(), node->get_clock()),
|
||||
compositeTask_("Sync status"),
|
||||
lastCallbackCalledStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1),
|
||||
targetFrequency_(0.0),
|
||||
diagnosticUpdater_(node, 2.0),
|
||||
inFrequencyStatus_(diagnostic_updater::FrequencyStatusParam(&inTargetFrequency_, &inTargetFrequency_, tolerance), node->get_clock()),
|
||||
inTimeStampStatus_(diagnostic_updater::TimeStampStatusParam(), node->get_clock()),
|
||||
outFrequencyStatus_(diagnostic_updater::FrequencyStatusParam(&outTargetFrequency_, &outTargetFrequency_, tolerance), node->get_clock()),
|
||||
outTimeStampStatus_(diagnostic_updater::TimeStampStatusParam(), node->get_clock()),
|
||||
inCompositeTask_("Input Status"),
|
||||
outCompositeTask_("Output Status"),
|
||||
lastTickInputStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1),
|
||||
lastTickOutputStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1),
|
||||
inTargetFrequency_(0.0),
|
||||
outTargetFrequency_(0.0),
|
||||
windowSize_(windowSize)
|
||||
{
|
||||
UASSERT(windowSize_ >= 1);
|
||||
@@ -41,71 +47,120 @@ class SyncDiagnostic {
|
||||
// Assuming format is /back_camera/left/image, we want "back_camera"
|
||||
strList.pop_back();
|
||||
}
|
||||
compositeTask_.addTask(&frequencyStatus_);
|
||||
compositeTask_.addTask(&timeStampStatus_);
|
||||
diagnosticUpdater_.add(compositeTask_);
|
||||
inCompositeTask_.addTask(&inFrequencyStatus_);
|
||||
inCompositeTask_.addTask(&inTimeStampStatus_);
|
||||
diagnosticUpdater_.add(inCompositeTask_);
|
||||
outCompositeTask_.addTask(&outFrequencyStatus_);
|
||||
outCompositeTask_.addTask(&outTimeStampStatus_);
|
||||
diagnosticUpdater_.add(outCompositeTask_);
|
||||
for(size_t i=0; i<otherTasks.size(); ++i)
|
||||
{
|
||||
diagnosticUpdater_.add(*otherTasks[i]);
|
||||
}
|
||||
diagnosticUpdater_.setHardwareID(strList.empty()?"none":uJoin(strList, "/"));
|
||||
diagnosticUpdater_.force_update();
|
||||
diagnosticTimer_ = node_->create_wall_timer(1s, std::bind(&SyncDiagnostic::diagnosticTimerCallback, this), nullptr);
|
||||
diagnosticTimer_ = node_->create_wall_timer(5s, std::bind(&SyncDiagnostic::diagnosticTimerCallback, this), nullptr);
|
||||
}
|
||||
|
||||
void tick(const rclcpp::Time & stamp, double targetFrequency = 0)
|
||||
void tickInput(const rclcpp::Time & stamp, double expectedFrequency = 0)
|
||||
{
|
||||
frequencyStatus_.tick();
|
||||
timeStampStatus_.tick(stamp);
|
||||
double singlePeriod = rtabmap_conversions::timestampFromROS(stamp) - lastCallbackCalledStamp_;
|
||||
updateFrequency(
|
||||
stamp,
|
||||
expectedFrequency,
|
||||
inFrequencyStatus_,
|
||||
inTimeStampStatus_,
|
||||
inWindow_,
|
||||
inTargetFrequency_,
|
||||
lastTickInputStamp_);
|
||||
}
|
||||
|
||||
window_.push_back(singlePeriod);
|
||||
if(window_.size() > windowSize_)
|
||||
{
|
||||
window_.pop_front();
|
||||
}
|
||||
double period = 0.0;
|
||||
if(window_.size() == windowSize_)
|
||||
{
|
||||
for(size_t i=0; i<window_.size(); ++i)
|
||||
{
|
||||
period += window_[i];
|
||||
}
|
||||
period /= windowSize_;
|
||||
}
|
||||
|
||||
if(period>0.0 && targetFrequency == 0 && (targetFrequency_ == 0.0 || period < 1.0/targetFrequency_))
|
||||
{
|
||||
targetFrequency_ = 1.0/period;
|
||||
}
|
||||
else if(targetFrequency>0)
|
||||
{
|
||||
targetFrequency_ = targetFrequency;
|
||||
}
|
||||
lastCallbackCalledStamp_ = rtabmap_conversions::timestampFromROS(stamp);
|
||||
void tickOutput(const rclcpp::Time & stamp, double expectedFrequency = 0)
|
||||
{
|
||||
updateFrequency(
|
||||
stamp,
|
||||
expectedFrequency,
|
||||
outFrequencyStatus_,
|
||||
outTimeStampStatus_,
|
||||
outWindow_,
|
||||
outTargetFrequency_,
|
||||
lastTickOutputStamp_);
|
||||
}
|
||||
|
||||
private:
|
||||
void diagnosticTimerCallback()
|
||||
{
|
||||
if(rtabmap_conversions::timestampFromROS(node_->now())-lastCallbackCalledStamp_ >= 5 && !topicsNotReceivedWarningMsg_.empty())
|
||||
UScopeMutex lock(tickMutex_);
|
||||
if(rtabmap_conversions::timestampFromROS(node_->now())-lastTickInputStamp_ >= 5 && !topicsNotReceivedWarningMsg_.empty())
|
||||
{
|
||||
RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, "%s", topicsNotReceivedWarningMsg_.c_str());
|
||||
RCLCPP_WARN(node_->get_logger(), "%s", topicsNotReceivedWarningMsg_.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
void updateFrequency(
|
||||
const rclcpp::Time & stamp,
|
||||
const double & expectedFrequency,
|
||||
diagnostic_updater::FrequencyStatus & freqStatus,
|
||||
diagnostic_updater::TimeStampStatus & timeStatus,
|
||||
std::deque<double> & window,
|
||||
double & targetFrequency,
|
||||
double & lastTickStamp)
|
||||
{
|
||||
UScopeMutex lock(tickMutex_);
|
||||
|
||||
freqStatus.tick();
|
||||
timeStatus.tick(stamp);
|
||||
|
||||
double stampSec = rtabmap_conversions::timestampFromROS(stamp);
|
||||
double singlePeriod = stampSec - lastTickStamp;
|
||||
|
||||
window.push_back(singlePeriod);
|
||||
if(window.size() > windowSize_)
|
||||
{
|
||||
window.pop_front();
|
||||
|
||||
double period = 0.0;
|
||||
if(window.size() == windowSize_)
|
||||
{
|
||||
for(size_t i=0; i<window.size(); ++i)
|
||||
{
|
||||
period += window[i];
|
||||
}
|
||||
period /= windowSize_;
|
||||
}
|
||||
|
||||
if(period>0.0 && expectedFrequency == 0 && (targetFrequency == 0.0 || period < 1.0/targetFrequency))
|
||||
{
|
||||
targetFrequency = 1.0/period;
|
||||
}
|
||||
else if(expectedFrequency>0)
|
||||
{
|
||||
targetFrequency = expectedFrequency;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
lastTickStamp = stampSec;
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Node * node_;
|
||||
std::string topicsNotReceivedWarningMsg_;
|
||||
diagnostic_updater::Updater diagnosticUpdater_;
|
||||
diagnostic_updater::FrequencyStatus frequencyStatus_;
|
||||
diagnostic_updater::TimeStampStatus timeStampStatus_;
|
||||
diagnostic_updater::CompositeDiagnosticTask compositeTask_;
|
||||
diagnostic_updater::FrequencyStatus inFrequencyStatus_;
|
||||
diagnostic_updater::TimeStampStatus inTimeStampStatus_;
|
||||
diagnostic_updater::FrequencyStatus outFrequencyStatus_;
|
||||
diagnostic_updater::TimeStampStatus outTimeStampStatus_;
|
||||
diagnostic_updater::CompositeDiagnosticTask inCompositeTask_;
|
||||
diagnostic_updater::CompositeDiagnosticTask outCompositeTask_;
|
||||
rclcpp::TimerBase::SharedPtr diagnosticTimer_;
|
||||
double lastCallbackCalledStamp_;
|
||||
double targetFrequency_;
|
||||
double lastTickInputStamp_;
|
||||
double lastTickOutputStamp_;
|
||||
double inTargetFrequency_;
|
||||
double outTargetFrequency_;
|
||||
int windowSize_;
|
||||
std::deque<double> window_;
|
||||
std::deque<double> inWindow_;
|
||||
std::deque<double> outWindow_;
|
||||
UMutex tickMutex_;
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -61,6 +61,7 @@ private:
|
||||
double depthScale_;
|
||||
int decimation_;
|
||||
double compressedRate_;
|
||||
double approxSyncMaxInterval_;
|
||||
|
||||
rclcpp::Time lastCompressedPublished_;
|
||||
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>rtabmap_sync</name>
|
||||
<version>0.21.5</version>
|
||||
<version>0.21.9</version>
|
||||
<description>RTAB-Map's synchronization package.</description>
|
||||
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
|
||||
<author>Mathieu Labbe</author>
|
||||
|
||||
@@ -31,7 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
namespace rtabmap_sync {
|
||||
|
||||
CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) :
|
||||
topicQueueSize_(1),
|
||||
topicQueueSize_(10),
|
||||
syncQueueSize_(10),
|
||||
approxSync_(true),
|
||||
subscribedToDepth_(!gui),
|
||||
@@ -363,6 +363,8 @@ CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) :
|
||||
{
|
||||
name_ = node.get_name();
|
||||
|
||||
syncCallbackGroup_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
|
||||
|
||||
// ROS related parameters (private)
|
||||
// ros2: should be declared in the constructor to be used by inherited classes in their constructor
|
||||
subscribedToDepth_ = node.declare_parameter("subscribe_depth", subscribedToDepth_);
|
||||
@@ -391,7 +393,7 @@ CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) :
|
||||
}
|
||||
syncQueueSize_ = node.declare_parameter("sync_queue_size", syncQueueSize_);
|
||||
|
||||
int qos = node.declare_parameter("qos", 0);
|
||||
int qos = node.declare_parameter("qos", (int)RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT);
|
||||
int qosOdom = node.declare_parameter("qos_odom", qos);
|
||||
int qosImage = node.declare_parameter("qos_image", qos);
|
||||
int qosCameraInfo = node.declare_parameter("qos_camera_info", qosImage);
|
||||
@@ -539,11 +541,15 @@ void CommonDataSubscriber::setupCallbacks(
|
||||
RCLCPP_INFO(node.get_logger(), "%s: qos_user_data = %d", name_.c_str(), qosUserData_);
|
||||
RCLCPP_INFO(node.get_logger(), "%s: approx_sync = %s", name_.c_str(), approxSync_?"true":"false");
|
||||
|
||||
rclcpp::SubscriptionOptions callbackOptions;
|
||||
callbackOptions.callback_group = syncCallbackGroup_;
|
||||
|
||||
subscribedToOdom_ = odomFrameId_.empty() && subscribedToOdom_;
|
||||
if(subscribedToDepth_)
|
||||
{
|
||||
setupDepthCallbacks(
|
||||
node,
|
||||
callbackOptions,
|
||||
subscribedToOdom_,
|
||||
subscribedToUserData_,
|
||||
subscribedToScan2d_,
|
||||
@@ -555,6 +561,7 @@ void CommonDataSubscriber::setupCallbacks(
|
||||
{
|
||||
setupStereoCallbacks(
|
||||
node,
|
||||
callbackOptions,
|
||||
subscribedToOdom_,
|
||||
subscribedToOdomInfo_);
|
||||
}
|
||||
@@ -562,6 +569,7 @@ void CommonDataSubscriber::setupCallbacks(
|
||||
{
|
||||
setupRGBCallbacks(
|
||||
node,
|
||||
callbackOptions,
|
||||
subscribedToOdom_,
|
||||
subscribedToUserData_,
|
||||
subscribedToScan2d_,
|
||||
@@ -583,6 +591,7 @@ void CommonDataSubscriber::setupCallbacks(
|
||||
|
||||
setupRGBD6Callbacks(
|
||||
node,
|
||||
callbackOptions,
|
||||
subscribedToOdom_,
|
||||
subscribedToUserData_,
|
||||
subscribedToScan2d_,
|
||||
@@ -594,6 +603,7 @@ void CommonDataSubscriber::setupCallbacks(
|
||||
{
|
||||
setupRGBD5Callbacks(
|
||||
node,
|
||||
callbackOptions,
|
||||
subscribedToOdom_,
|
||||
subscribedToUserData_,
|
||||
subscribedToScan2d_,
|
||||
@@ -605,6 +615,7 @@ void CommonDataSubscriber::setupCallbacks(
|
||||
{
|
||||
setupRGBD4Callbacks(
|
||||
node,
|
||||
callbackOptions,
|
||||
subscribedToOdom_,
|
||||
subscribedToUserData_,
|
||||
subscribedToScan2d_,
|
||||
@@ -616,6 +627,7 @@ void CommonDataSubscriber::setupCallbacks(
|
||||
{
|
||||
setupRGBD3Callbacks(
|
||||
node,
|
||||
callbackOptions,
|
||||
subscribedToOdom_,
|
||||
subscribedToUserData_,
|
||||
subscribedToScan2d_,
|
||||
@@ -627,6 +639,7 @@ void CommonDataSubscriber::setupCallbacks(
|
||||
{
|
||||
setupRGBD2Callbacks(
|
||||
node,
|
||||
callbackOptions,
|
||||
subscribedToOdom_,
|
||||
subscribedToUserData_,
|
||||
subscribedToScan2d_,
|
||||
@@ -647,6 +660,7 @@ void CommonDataSubscriber::setupCallbacks(
|
||||
{
|
||||
setupRGBDXCallbacks(
|
||||
node,
|
||||
callbackOptions,
|
||||
subscribedToOdom_,
|
||||
subscribedToUserData_,
|
||||
subscribedToScan2d_,
|
||||
@@ -658,6 +672,7 @@ void CommonDataSubscriber::setupCallbacks(
|
||||
{
|
||||
setupRGBDCallbacks(
|
||||
node,
|
||||
callbackOptions,
|
||||
subscribedToOdom_,
|
||||
subscribedToUserData_,
|
||||
subscribedToScan2d_,
|
||||
@@ -670,6 +685,7 @@ void CommonDataSubscriber::setupCallbacks(
|
||||
{
|
||||
setupScanCallbacks(
|
||||
node,
|
||||
callbackOptions,
|
||||
subscribedToScan2d_,
|
||||
subscribedToScanDescriptor_,
|
||||
subscribedToOdom_,
|
||||
@@ -680,6 +696,7 @@ void CommonDataSubscriber::setupCallbacks(
|
||||
{
|
||||
setupSensorDataCallbacks(
|
||||
node,
|
||||
callbackOptions,
|
||||
subscribedToOdom_,
|
||||
subscribedToOdomInfo_);
|
||||
}
|
||||
@@ -687,6 +704,7 @@ void CommonDataSubscriber::setupCallbacks(
|
||||
{
|
||||
setupOdomCallbacks(
|
||||
node,
|
||||
callbackOptions,
|
||||
subscribedToUserData_,
|
||||
subscribedToOdomInfo_);
|
||||
}
|
||||
@@ -699,8 +717,12 @@ void CommonDataSubscriber::setupCallbacks(
|
||||
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
|
||||
"published (\"$ ros2 topic hz my_topic\") and the timestamps in their "
|
||||
"header are set. If topics are coming from different computers, make sure "
|
||||
"the clocks of the computers are synchronized (\"ntpdate\"). %s%s",
|
||||
"the clocks of the computers are synchronized (\"ntpdate\"). Ajusting "
|
||||
"topic_queue_size (%d) and sync_queue_size (%d) can also help for better "
|
||||
"synchronization if framerates and/or delays are different. %s%s",
|
||||
name_.c_str(),
|
||||
topicQueueSize_,
|
||||
syncQueueSize_,
|
||||
approxSync_?
|
||||
uFormat("If topics are not published at the same rate, you could increase \"sync_queue_size\" and/or \"topic_queue_size\" parameters (current=%d and %d respectively).", syncQueueSize_, topicQueueSize_).c_str():
|
||||
"Parameter \"approx_sync\" is false, which means that input topics should have all the exact timestamp for the callback to be called.",
|
||||
@@ -1082,7 +1104,7 @@ void CommonDataSubscriber::tick(const rclcpp::Time & stamp, double targetFrequen
|
||||
{
|
||||
if(syncDiagnostic_.get())
|
||||
{
|
||||
syncDiagnostic_->tick(stamp, targetFrequency);
|
||||
syncDiagnostic_->tickOutput(stamp, targetFrequency);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -35,6 +35,7 @@ void CommonDataSubscriber::depthCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
@@ -48,6 +49,7 @@ void CommonDataSubscriber::depthScan2dCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
@@ -60,6 +62,7 @@ void CommonDataSubscriber::depthScan3dCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
@@ -72,6 +75,7 @@ void CommonDataSubscriber::depthScanDescCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
@@ -88,6 +92,7 @@ void CommonDataSubscriber::depthInfoCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
@@ -101,6 +106,7 @@ void CommonDataSubscriber::depthScan2dInfoCallback(
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
@@ -113,6 +119,7 @@ void CommonDataSubscriber::depthScan3dInfoCallback(
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
@@ -125,6 +132,7 @@ void CommonDataSubscriber::depthScanDescInfoCallback(
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
@@ -142,6 +150,7 @@ void CommonDataSubscriber::depthOdomCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
@@ -155,6 +164,7 @@ void CommonDataSubscriber::depthOdomScan2dCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
@@ -167,6 +177,7 @@ void CommonDataSubscriber::depthOdomScan3dCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
@@ -179,6 +190,7 @@ void CommonDataSubscriber::depthOdomScanDescCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
@@ -195,6 +207,7 @@ void CommonDataSubscriber::depthOdomInfoCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
@@ -208,6 +221,7 @@ void CommonDataSubscriber::depthOdomScan2dInfoCallback(
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
@@ -220,6 +234,7 @@ void CommonDataSubscriber::depthOdomScan3dInfoCallback(
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
@@ -232,6 +247,7 @@ void CommonDataSubscriber::depthOdomScanDescInfoCallback(
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
@@ -249,6 +265,7 @@ void CommonDataSubscriber::depthDataCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
@@ -262,6 +279,7 @@ void CommonDataSubscriber::depthDataScan2dCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
@@ -274,6 +292,7 @@ void CommonDataSubscriber::depthDataScan3dCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
@@ -286,6 +305,7 @@ void CommonDataSubscriber::depthDataScanDescCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
@@ -302,6 +322,7 @@ void CommonDataSubscriber::depthDataInfoCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
@@ -315,6 +336,7 @@ void CommonDataSubscriber::depthDataScan2dInfoCallback(
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
@@ -327,6 +349,7 @@ void CommonDataSubscriber::depthDataScan3dInfoCallback(
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
@@ -339,6 +362,7 @@ void CommonDataSubscriber::depthDataScanDescInfoCallback(
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
@@ -356,6 +380,7 @@ void CommonDataSubscriber::depthOdomDataCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr depthMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
@@ -369,6 +394,7 @@ void CommonDataSubscriber::depthOdomDataScan2dCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
@@ -381,6 +407,7 @@ void CommonDataSubscriber::depthOdomDataScan3dCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
@@ -393,6 +420,7 @@ void CommonDataSubscriber::depthOdomDataScanDescCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
@@ -409,6 +437,7 @@ void CommonDataSubscriber::depthOdomDataInfoCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
|
||||
@@ -422,6 +451,7 @@ void CommonDataSubscriber::depthOdomDataScan2dInfoCallback(
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
}
|
||||
@@ -434,6 +464,7 @@ void CommonDataSubscriber::depthOdomDataScan3dInfoCallback(
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), cv_bridge::toCvShare(depthMsg), *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
}
|
||||
@@ -446,6 +477,7 @@ void CommonDataSubscriber::depthOdomDataScanDescInfoCallback(
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
{
|
||||
@@ -457,6 +489,7 @@ void CommonDataSubscriber::depthOdomDataScanDescInfoCallback(
|
||||
|
||||
void CommonDataSubscriber::setupDepthCallbacks(
|
||||
rclcpp::Node& node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
bool subscribeUserData,
|
||||
@@ -471,24 +504,24 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
RCLCPP_INFO(node.get_logger(), "Setup depth callback");
|
||||
|
||||
image_transport::TransportHints hints(&node);
|
||||
imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
|
||||
imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
|
||||
cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile());
|
||||
imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options);
|
||||
imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options);
|
||||
cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile(), options);
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -499,11 +532,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -514,11 +547,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -529,7 +562,7 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, depthOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -541,16 +574,16 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
#endif
|
||||
if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -561,11 +594,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -576,11 +609,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, depthOdomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -591,7 +624,7 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, depthOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -602,17 +635,17 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -623,12 +656,12 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -639,11 +672,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, depthDataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -654,7 +687,7 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, depthDataInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -668,11 +701,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -683,11 +716,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -698,11 +731,11 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, depthScan3dInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -713,7 +746,7 @@ void CommonDataSubscriber::setupDepthCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, depthInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
|
||||
@@ -32,6 +32,7 @@ namespace rtabmap_sync {
|
||||
void CommonDataSubscriber::odomCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(odomMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
@@ -41,6 +42,7 @@ void CommonDataSubscriber::odomInfoCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(odomMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan::SharedPtr scan2dMsg; // Null
|
||||
commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
|
||||
@@ -50,6 +52,7 @@ void CommonDataSubscriber::odomDataCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(odomMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
|
||||
}
|
||||
@@ -58,6 +61,7 @@ void CommonDataSubscriber::odomDataInfoCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(odomMsg->header.stamp);}
|
||||
sensor_msgs::msg::PointCloud2::SharedPtr scan3dMsg; // Null
|
||||
commonOdomCallback(odomMsg, userDataMsg, odomInfoMsg);
|
||||
}
|
||||
@@ -65,6 +69,7 @@ void CommonDataSubscriber::odomDataInfoCallback(
|
||||
|
||||
void CommonDataSubscriber::setupOdomCallbacks(
|
||||
rclcpp::Node& node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeUserData,
|
||||
bool subscribeOdomInfo)
|
||||
{
|
||||
@@ -72,16 +77,16 @@ void CommonDataSubscriber::setupOdomCallbacks(
|
||||
|
||||
if(subscribeUserData || subscribeOdomInfo)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL3(CommonDataSubscriber, odomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -94,7 +99,7 @@ void CommonDataSubscriber::setupOdomCallbacks(
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync_, syncQueueSize_, odomSub_, odomInfoSub_);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -34,6 +34,7 @@ void CommonDataSubscriber::rgbCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
@@ -47,6 +48,7 @@ void CommonDataSubscriber::rgbScan2dCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
@@ -59,6 +61,7 @@ void CommonDataSubscriber::rgbScan3dCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
@@ -71,6 +74,7 @@ void CommonDataSubscriber::rgbScanDescCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
@@ -87,6 +91,7 @@ void CommonDataSubscriber::rgbInfoCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
@@ -100,6 +105,7 @@ void CommonDataSubscriber::rgbScan2dInfoCallback(
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
@@ -112,6 +118,7 @@ void CommonDataSubscriber::rgbScan3dInfoCallback(
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
@@ -124,6 +131,7 @@ void CommonDataSubscriber::rgbScanDescInfoCallback(
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
@@ -141,6 +149,7 @@ void CommonDataSubscriber::rgbOdomCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
@@ -154,6 +163,7 @@ void CommonDataSubscriber::rgbOdomScan2dCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
@@ -166,6 +176,7 @@ void CommonDataSubscriber::rgbOdomScan3dCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
@@ -178,6 +189,7 @@ void CommonDataSubscriber::rgbOdomScanDescCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
@@ -194,6 +206,7 @@ void CommonDataSubscriber::rgbOdomInfoCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
@@ -207,6 +220,7 @@ void CommonDataSubscriber::rgbOdomScan2dInfoCallback(
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
@@ -219,6 +233,7 @@ void CommonDataSubscriber::rgbOdomScan3dInfoCallback(
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
@@ -231,6 +246,7 @@ void CommonDataSubscriber::rgbOdomScanDescInfoCallback(
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::UserData::SharedPtr userDataMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
@@ -248,6 +264,7 @@ void CommonDataSubscriber::rgbDataCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // Null
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
@@ -261,6 +278,7 @@ void CommonDataSubscriber::rgbDataScan2dCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
@@ -273,6 +291,7 @@ void CommonDataSubscriber::rgbDataScan3dCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
@@ -285,6 +304,7 @@ void CommonDataSubscriber::rgbDataScanDescCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::ConstSharedPtr odomMsg; // null
|
||||
rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
@@ -301,6 +321,7 @@ void CommonDataSubscriber::rgbDataInfoCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
@@ -314,6 +335,7 @@ void CommonDataSubscriber::rgbDataScan2dInfoCallback(
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
@@ -326,6 +348,7 @@ void CommonDataSubscriber::rgbDataScan3dInfoCallback(
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
@@ -338,6 +361,7 @@ void CommonDataSubscriber::rgbDataScanDescInfoCallback(
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
nav_msgs::msg::Odometry::SharedPtr odomMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
@@ -355,6 +379,7 @@ void CommonDataSubscriber::rgbOdomDataCallback(
|
||||
const sensor_msgs::msg::Image::ConstSharedPtr imageMsg,
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::LaserScan scanMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
@@ -368,6 +393,7 @@ void CommonDataSubscriber::rgbOdomDataScan2dCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
@@ -380,6 +406,7 @@ void CommonDataSubscriber::rgbOdomDataScan3dCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
@@ -392,6 +419,7 @@ void CommonDataSubscriber::rgbOdomDataScanDescCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
rtabmap_msgs::msg::OdomInfo::SharedPtr odomInfoMsg; // null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
@@ -408,6 +436,7 @@ void CommonDataSubscriber::rgbOdomDataInfoCallback(
|
||||
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
@@ -421,6 +450,7 @@ void CommonDataSubscriber::rgbOdomDataScan2dInfoCallback(
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::PointCloud2 scan3dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, *scanMsg, scan3dMsg, odomInfoMsg);
|
||||
@@ -433,6 +463,7 @@ void CommonDataSubscriber::rgbOdomDataScan3dInfoCallback(
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
sensor_msgs::msg::LaserScan scan2dMsg; // Null
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
commonSingleCameraCallback(odomMsg, userDataMsg, cv_bridge::toCvShare(imageMsg), depthMsg, *cameraInfoMsg, *cameraInfoMsg, scan2dMsg, *scanMsg, odomInfoMsg);
|
||||
@@ -445,6 +476,7 @@ void CommonDataSubscriber::rgbOdomDataScanDescInfoCallback(
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanMsg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(imageMsg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr depthMsg;// Null
|
||||
std::vector<rtabmap_msgs::msg::GlobalDescriptor> globalDescriptor;
|
||||
if(!scanMsg->global_descriptor.data.empty())
|
||||
@@ -457,6 +489,7 @@ void CommonDataSubscriber::rgbOdomDataScanDescInfoCallback(
|
||||
|
||||
void CommonDataSubscriber::setupRGBCallbacks(
|
||||
rclcpp::Node& node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
bool subscribeUserData,
|
||||
@@ -471,23 +504,23 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
RCLCPP_INFO(node.get_logger(), "Setup rgb-only callback");
|
||||
|
||||
image_transport::TransportHints hints(&node);
|
||||
imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
|
||||
cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile());
|
||||
imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options);
|
||||
cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile(), options);
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -498,11 +531,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -513,11 +546,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -528,7 +561,7 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -540,16 +573,16 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
#endif
|
||||
if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -560,11 +593,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -575,11 +608,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -590,7 +623,7 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -601,17 +634,17 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -622,12 +655,12 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -638,11 +671,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -653,7 +686,7 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -667,11 +700,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -682,11 +715,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -697,11 +730,11 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -712,7 +745,7 @@ void CommonDataSubscriber::setupRGBCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, odomInfoSub_);
|
||||
}
|
||||
else
|
||||
|
||||
@@ -36,6 +36,7 @@ namespace rtabmap_sync {
|
||||
void CommonDataSubscriber::rgbdCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
@@ -61,6 +62,7 @@ void CommonDataSubscriber::rgbdScan2dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
@@ -85,6 +87,7 @@ void CommonDataSubscriber::rgbdScan3dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
@@ -109,6 +112,7 @@ void CommonDataSubscriber::rgbdScanDescCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
@@ -132,6 +136,7 @@ void CommonDataSubscriber::rgbdInfoCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
@@ -158,6 +163,7 @@ void CommonDataSubscriber::rgbdOdomCallback(
|
||||
const nav_msgs::msg::Odometry::ConstSharedPtr odomMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
@@ -183,6 +189,7 @@ void CommonDataSubscriber::rgbdOdomScan2dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
@@ -207,6 +214,7 @@ void CommonDataSubscriber::rgbdOdomScan3dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
@@ -231,6 +239,7 @@ void CommonDataSubscriber::rgbdOdomScanDescCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
@@ -258,6 +267,7 @@ void CommonDataSubscriber::rgbdOdomInfoCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
@@ -284,6 +294,7 @@ void CommonDataSubscriber::rgbdDataCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
@@ -309,6 +320,7 @@ void CommonDataSubscriber::rgbdDataScan2dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
@@ -333,6 +345,7 @@ void CommonDataSubscriber::rgbdDataScan3dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
@@ -357,6 +370,7 @@ void CommonDataSubscriber::rgbdDataScanDescCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
@@ -384,6 +398,7 @@ void CommonDataSubscriber::rgbdDataInfoCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
@@ -410,6 +425,7 @@ void CommonDataSubscriber::rgbdOdomDataCallback(
|
||||
const rtabmap_msgs::msg::UserData::ConstSharedPtr userDataMsg,
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
@@ -435,6 +451,7 @@ void CommonDataSubscriber::rgbdOdomDataScan2dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::LaserScan::ConstSharedPtr scanMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
@@ -459,6 +476,7 @@ void CommonDataSubscriber::rgbdOdomDataScan3dCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const sensor_msgs::msg::PointCloud2::ConstSharedPtr scan3dMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
@@ -483,6 +501,7 @@ void CommonDataSubscriber::rgbdOdomDataScanDescCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::ScanDescriptor::ConstSharedPtr scanDescMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
@@ -510,6 +529,7 @@ void CommonDataSubscriber::rgbdOdomDataInfoCallback(
|
||||
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1Msg,
|
||||
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr odomInfoMsg)
|
||||
{
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);}
|
||||
cv_bridge::CvImageConstPtr rgb, depth;
|
||||
rtabmap_conversions::toCvShare(image1Msg, rgb, depth);
|
||||
|
||||
@@ -531,6 +551,7 @@ void CommonDataSubscriber::rgbdOdomDataInfoCallback(
|
||||
|
||||
void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
rclcpp::Node& node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
bool subscribeUserData,
|
||||
@@ -555,17 +576,17 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
{
|
||||
rgbdSubs_.resize(1);
|
||||
rgbdSubs_[0] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
|
||||
rgbdSubs_[0]->subscribe(&node, "rgbd_image", rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
|
||||
rgbdSubs_[0]->subscribe(&node, "rgbd_image", rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options);
|
||||
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -576,7 +597,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -587,7 +608,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -598,7 +619,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -610,11 +631,11 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
#endif
|
||||
if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -625,7 +646,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -636,7 +657,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -647,7 +668,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -658,11 +679,11 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -673,7 +694,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -684,7 +705,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -695,7 +716,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -709,7 +730,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -720,7 +741,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -731,7 +752,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -742,7 +763,7 @@ void CommonDataSubscriber::setupRGBDCallbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
|
||||
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
namespace rtabmap_sync {
|
||||
|
||||
#define IMAGE_CONVERSION() \
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} \
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2); \
|
||||
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2); \
|
||||
rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
|
||||
@@ -343,6 +344,7 @@ void CommonDataSubscriber::rgbd2OdomDataInfoCallback(
|
||||
|
||||
void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
rclcpp::Node& node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
bool subscribeUserData,
|
||||
@@ -360,17 +362,17 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
for(int i=0; i<2; ++i)
|
||||
{
|
||||
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
|
||||
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
|
||||
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options);
|
||||
}
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -381,7 +383,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -392,7 +394,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -403,7 +405,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -415,11 +417,11 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
#endif
|
||||
if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -430,7 +432,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -441,7 +443,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -452,7 +454,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -463,11 +465,11 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -478,7 +480,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -489,7 +491,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -500,7 +502,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -514,7 +516,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -525,7 +527,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -536,7 +538,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -547,7 +549,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
|
||||
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
namespace rtabmap_sync {
|
||||
|
||||
#define IMAGE_CONVERSION() \
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} \
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(3); \
|
||||
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(3); \
|
||||
rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
|
||||
@@ -431,6 +432,7 @@ void CommonDataSubscriber::rgbd3OdomDataInfoCallback(
|
||||
|
||||
void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
rclcpp::Node& node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
bool subscribeUserData,
|
||||
@@ -448,17 +450,17 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
for(int i=0; i<3; ++i)
|
||||
{
|
||||
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
|
||||
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
|
||||
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options);
|
||||
}
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDescriptor)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -469,7 +471,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -480,7 +482,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -491,7 +493,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -503,11 +505,11 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
#endif
|
||||
if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDescriptor)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -518,7 +520,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -529,7 +531,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -540,7 +542,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -551,11 +553,11 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDescriptor)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -566,7 +568,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -577,7 +579,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -588,7 +590,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -602,7 +604,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
if(subscribeScanDescriptor)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -613,7 +615,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -624,7 +626,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
@@ -636,7 +638,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
|
||||
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
namespace rtabmap_sync {
|
||||
|
||||
#define IMAGE_CONVERSION() \
|
||||
if(syncDiagnostic_.get()) {syncDiagnostic_->tickInput(image1Msg->header.stamp);} \
|
||||
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(4); \
|
||||
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(4); \
|
||||
rtabmap_conversions::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
|
||||
@@ -400,6 +401,7 @@ void CommonDataSubscriber::rgbd4OdomDataInfoCallback(
|
||||
|
||||
void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
rclcpp::Node& node,
|
||||
const rclcpp::SubscriptionOptions & options,
|
||||
bool subscribeOdom,
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
bool subscribeUserData,
|
||||
@@ -417,17 +419,17 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
for(int i=0; i<4; ++i)
|
||||
{
|
||||
rgbdSubs_[i] = new message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>;
|
||||
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile());
|
||||
rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options);
|
||||
}
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
if(subscribeOdom && subscribeUserData)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -438,7 +440,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -449,7 +451,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -460,7 +462,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -472,11 +474,11 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
#endif
|
||||
if(subscribeOdom)
|
||||
{
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -487,7 +489,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -498,7 +500,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -509,7 +511,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -520,11 +522,11 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
#ifdef RTABMAP_SYNC_USER_DATA
|
||||
else if(subscribeUserData)
|
||||
{
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile());
|
||||
userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options);
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -535,7 +537,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -546,7 +548,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -557,7 +559,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
@@ -571,7 +573,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
if(subscribeScanDesc)
|
||||
{
|
||||
subscribedToScanDescriptor_ = true;
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -582,7 +584,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
else if(subscribeScan2d)
|
||||
{
|
||||
subscribedToScan2d_ = true;
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -593,7 +595,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
else if(subscribeScan3d)
|
||||
{
|
||||
subscribedToScan3d_ = true;
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile());
|
||||
scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options);
|
||||
if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = false;
|
||||
@@ -604,7 +606,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks(
|
||||
else if(subscribeOdomInfo)
|
||||
{
|
||||
subscribedToOdomInfo_ = true;
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile());
|
||||
odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options);
|
||||
SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_);
|
||||
}
|
||||
else
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user