From 0e81a94c4c585c422cc35a9c16db957a8a79bd31 Mon Sep 17 00:00:00 2001 From: Jaiveer Singh Date: Wed, 5 Apr 2023 18:02:27 -0700 Subject: [PATCH] Isaac ROS 0.30.0 (DP3) --- .gitattributes | 3 + README.md | 174 +++--- docs/elbrus-slam.md | 13 +- docs/tutorial-isaac-sim.md | 30 +- docs/tutorial-realsense.md | 267 +++++++++ isaac_ros_visual_slam/CMakeLists.txt | 38 +- .../isaac_ros_visual_slam_realsense.launch.py | 12 +- isaac_ros_visual_slam/package.xml | 6 +- .../params/ekf_dual_nvslam.yaml | 277 +++++++++ .../rviz/dual_realsense.cfg.rviz | 298 ++++++++++ isaac_ros_visual_slam/rviz/realsense.cfg.rviz | 539 ++++++++++++++++++ .../rviz/vslam_keepall.cfg.rviz | 375 ++++++++++++ .../src/visual_slam_node.cpp | 82 +-- .../CMakeLists.txt | 15 +- isaac_ros_visual_slam_interfaces/package.xml | 3 +- resources/Isaac_sim_enable_stereo.png | 4 +- resources/Isaac_sim_play.png | 3 + resources/Isaac_sim_set_stereo_offset.png | 4 +- resources/Isaac_sim_visual_slam.png | 3 - resources/RViz_0217-cube_vslam-keepall.gif | 3 + resources/RViz_0217-cube_vslam-keepall.png | 3 + ...M-and-RViz2-on-Jetson_X11forward-to-PC.png | 3 + resources/back_cam_transform.png | 3 + resources/dual_realsense.gif | 3 + resources/front_cam_transform.png | 3 + ...realsense-viewer_fw-update-recommended.png | 3 + resources/realsense.gif | 3 + resources/realsense_emitter.png | 3 + resources/realsense_viewer_serial_number.png | 3 + resources/vslam_odometry_nav2_diagram.png | 3 + 30 files changed, 2003 insertions(+), 176 deletions(-) create mode 100644 docs/tutorial-realsense.md create mode 100644 isaac_ros_visual_slam/params/ekf_dual_nvslam.yaml create mode 100644 isaac_ros_visual_slam/rviz/dual_realsense.cfg.rviz create mode 100644 isaac_ros_visual_slam/rviz/realsense.cfg.rviz create mode 100644 isaac_ros_visual_slam/rviz/vslam_keepall.cfg.rviz create mode 100644 resources/Isaac_sim_play.png delete mode 100644 resources/Isaac_sim_visual_slam.png create mode 100644 resources/RViz_0217-cube_vslam-keepall.gif create mode 100644 resources/RViz_0217-cube_vslam-keepall.png create mode 100644 resources/VSLAM-and-RViz2-on-Jetson_X11forward-to-PC.png create mode 100644 resources/back_cam_transform.png create mode 100644 resources/dual_realsense.gif create mode 100644 resources/front_cam_transform.png create mode 100644 resources/realsense-viewer_fw-update-recommended.png create mode 100644 resources/realsense.gif create mode 100644 resources/realsense_emitter.png create mode 100644 resources/realsense_viewer_serial_number.png create mode 100644 resources/vslam_odometry_nav2_diagram.png diff --git a/.gitattributes b/.gitattributes index e2579a4..816c561 100644 --- a/.gitattributes +++ b/.gitattributes @@ -1,3 +1,6 @@ +# Ignore Python files in linguist +*.py linguist-detectable=false + # Images *.gif filter=lfs diff=lfs merge=lfs -text *.jpg filter=lfs diff=lfs merge=lfs -text diff --git a/README.md b/README.md index 44710e8..88b8d26 100644 --- a/README.md +++ b/README.md @@ -3,36 +3,51 @@
--- + ## Webinar Available + Learn how to use this package by watching our on-demand webinar: [Pinpoint, 250 fps, ROS 2 Localization with vSLAM on Jetson](https://gateway.on24.com/wcc/experience/elitenvidiabrill/1407606/3998202/isaac-ros-webinar-series) --- ## Overview -This repository provides a ROS2 package that performs stereo visual simultaneous localization and mapping (VSLAM) and estimates stereo visual inertial odometry using the [Isaac Elbrus](https://docs.nvidia.com/isaac/packages/visual_slam/doc/elbrus_visual_slam.html) GPU-accelerated library. It takes in a time-synced pair of stereo images (grayscale) along with respective camera intrinsics to publish the current pose of the camera relative to its start pose. +This repository provides a high-performance, best-in-class ROS 2 package for VSLAM (visual simultaneous localization and mapping). This package uses a stereo camera with an IMU to estimate odometry as an input to navigation. It is GPU accelerated to provide real-time, low-latency results in a robotics application. VSLAM provides an additional odometry source for mobile robots (ground based) and can be the primary odometry source for drones. + +VSLAM provides a method for visually estimating the position of a robot relative to its start position, known as VO (visual odometry). This is particularly useful in environments where GPS is not available (such as indoors) or intermittent (such as urban locations with structures blocking line of sight to GPS satellites). This method is designed to use left and right stereo camera frames and an IMU (inertial measurement unit) as input. It uses input stereo image pairs to find matching key points in the left and right images; using the baseline between the left and right camera, it can estimate the distance to the key point. Using two consecutive input stereo image pairs, VSLAM can track the 3D motion of key points between the two consecutive images to estimate the 3D motion of the camera--which is then used to compute odometry as an output to navigation. Compared to the classic approach to VSLAM, this method uses GPU acceleration to find and match more key points in real-time, with fine tuning to minimize overall reprojection error. + +Key points depend on distinctive features in the left and right camera image that can be repeatedly detected with changes in size, orientation, perspective, lighting, and image noise. In some instances, the number of key points may be limited or entirely absent; for example, if the camera field of view is only looking at a large solid colored wall, no key points may be detected. If there are insufficient key points, this module uses motion sensed with the IMU to provide a sensor for motion, which, when measured, can provide an estimate for odometry. This method, known as VIO (visual-inertial odometry), improves estimation performance when there is a lack of distinctive features in the scene to track motion visually. + +SLAM (simultaneous localization and mapping) is built on top of VIO, creating a map of key points that can be used to determine if an area is previously seen. When VSLAM determines that an area is previously seen, it reduces uncertainty in the map estimate, which is known as loop closure. VSLAM uses a statistical approach to loop closure that is more compute efficient to provide a real time solution, improving convergence in loop closure. -Elbrus is based on two core technologies: Visual Odometry (VO) and Simultaneous Localization and Mapping (SLAM). +
-Visual SLAM is a method for estimating a camera position relative to its start position. This method has an iterative nature. At each iteration, it considers two consequential input frames (stereo pairs). On both the frames, it finds a set of keypoints. Matching keypoints in these two sets gives the ability to estimate the transition and relative rotation of the camera between frames. +There are multiple methods for estimating odometry as an input to navigation. None of these methods are perfect; each has limitations because of systematic flaws in the sensor providing measured observations, such as missing LIDAR returns absorbed by black surfaces, inaccurate wheel ticks when the wheel slips on the ground, or a lack of distinctive features in a scene limiting key points in a camera image. A practical approach to tracking odometry is to use multiple sensors with diverse methods so that systemic issues with one method can be compensated for by another method. With three separate estimates of odometry, failures in a single method can be detected, allowing for fusion of the multiple methods into a single higher quality result. VSLAM provides a vision- and IMU-based solution to estimating odometry that is different from the common practice of using LIDAR and wheel odometry. VSLAM can even be used to improve diversity, with multiple stereo cameras positioned in different directions to provide multiple, concurrent visual estimates of odometry. -Simultaneous Localization and Mapping is a method built on top of the VO predictions. It aims to improve the quality of VO estimations by leveraging the knowledge of previously seen parts of a trajectory. It detects if the current scene was seen in the past (i.e. a loop in camera movement) and runs an additional optimization procedure to tune previously obtained poses. +To learn more about VSLAM, refer to the [Elbrus SLAM](docs/elbrus-slam.md) documentation. -Along with visual data, Elbrus can optionally use Inertial Measurement Unit (IMU) measurements. It automatically switches to IMU when VO is unable to estimate a pose; for example, when there is dark lighting or long solid featureless surfaces in front of a camera. Elbrus delivers real-time tracking performance: more than 60 FPS for VGA resolution. For the KITTI benchmark, the algorithm achieves a drift of ~1% in localization and an orientation error of 0.003 degrees per meter of motion. Elbrus allows for robust tracking in various environments and with different use cases: indoor, outdoor, aerial, HMD, automotive, and robotics. +## Accuracy -To learn more about Elbrus SLAM click [here](docs/elbrus-slam.md). +VSLAM is a best-in-class package with the lowest translation and rotational error as measured on KITTI [Visual Odometry / SLAM Evaluation 2012](https://www.cvlibs.net/datasets/kitti/eval_odometry.php) for real-time applications. + +| Method | Runtime | Translation | Rotation | Platform | +| ------------------------------------------------- | ------- | ----------- | ------------ | ------------------------- | +| [VSLAM](docs/elbrus-slam.md) | 0.007s | 0.94% | 0.0019 deg/m | Jetson AGX Xavier aarch64 | +| [ORB-SLAM2](https://github.com/raulmur/ORB_SLAM2) | 0.06s | 1.15% | 0.0027 deg/m | 2 cores @ >3.5 Ghz x86_64 | + +In addition to results from standard benchmarks, we test loop closure for VSLAM on sequences of over 1000 meters, with coverage for indoor and outdoor scenes. ## Performance -The following are the benchmark performance results of the prepared pipelines in this package, by supported platform: +The following table summarizes the per-platform performance statistics of sample graphs that use this package, with links included to the full benchmark output. These benchmark configurations are taken from the [Isaac ROS Benchmark](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark#list-of-isaac-ros-benchmarks) collection, based on the [`ros2_benchmark`](https://github.com/NVIDIA-ISAAC-ROS/ros2_benchmark) framework. -| Pipeline | AGX Orin | Orin Nano | x86_64 w/ RTX 3060 Ti | -| -------- | ------------------ | ----------------- | --------------------- | -| VSLAM | 250 fps
3.1ms | 105 fps
10ms | 265 fps
5.2ms | +| Sample Graph | Input Size | AGX Orin | Orin NX | Orin Nano 8GB | x86_64 w/ RTX 3060 Ti | +| ------------------------------------------------------------------------------------------------------------------------------------ | ---------- | ---------------------------------------------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------- | +| [Visual SLAM Node](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/scripts//isaac_ros_visual_slam_node.py) | 720p | [228 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_visual_slam_node-agx_orin.json)
36 ms | [124 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_visual_slam_node-orin_nx.json)
39 ms | [116 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_visual_slam_node-orin_nano_8gb.json)
85 ms | [238 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_visual_slam_node-x86_64_rtx_3060Ti.json)
50 ms | -These data have been collected per the methodology described [here](https://github.com/NVIDIA-ISAAC-ROS/.github/blob/main/profile/performance-summary.md#methodology). ## Commercial Support & Source Access + For commercial support and access to source code, please [contact NVIDIA](https://developer.nvidia.com/isaac-platform-contact-us-form). ## Table of Contents @@ -40,6 +55,7 @@ For commercial support and access to source code, please [contact NVIDIA](https: - [Isaac ROS Visual SLAM](#isaac-ros-visual-slam) - [Webinar Available](#webinar-available) - [Overview](#overview) + - [Accuracy](#accuracy) - [Performance](#performance) - [Commercial Support \& Source Access](#commercial-support--source-access) - [Table of Contents](#table-of-contents) @@ -66,24 +82,24 @@ For commercial support and access to source code, please [contact NVIDIA](https: ## Latest Update -Update 2022-10-19: Updated OSS licensing +Update 2022-03-23: Update Elbrus library and performance improvements ## Supported Platforms -This package is designed and tested to be compatible with ROS2 Humble running on [Jetson](https://developer.nvidia.com/embedded-computing) or an x86_64 system with an NVIDIA GPU. +This package is designed and tested to be compatible with ROS 2 Humble running on [Jetson](https://developer.nvidia.com/embedded-computing) or an x86_64 system with an NVIDIA GPU. -> **Note**: Versions of ROS2 earlier than Humble are **not** supported. This package depends on specific ROS2 implementation features that were only introduced beginning with the Humble release. +> **Note**: Versions of ROS 2 earlier than Humble are **not** supported. This package depends on specific ROS 2 implementation features that were only introduced beginning with the Humble release. -| Platform | Hardware | Software | Notes | -| -------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| Jetson | [Jetson Orin](https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-orin/)
[Jetson Xavier](https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-agx-xavier/) | [JetPack 5.0.2](https://developer.nvidia.com/embedded/jetpack) | For best performance, ensure that [power settings](https://docs.nvidia.com/jetson/archives/r34.1/DeveloperGuide/text/SD/PlatformPowerAndPerformance.html) are configured appropriately. | -| x86_64 | NVIDIA GPU | [Ubuntu 20.04+](https://releases.ubuntu.com/20.04/)
[CUDA 11.6.1+](https://developer.nvidia.com/cuda-downloads) | +| Platform | Hardware | Software | Notes | +| -------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------ | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| Jetson | [Jetson Orin](https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-orin/)
[Jetson Xavier](https://www.nvidia.com/en-us/autonomous-machines/embedded-systems/jetson-agx-xavier/) | [JetPack 5.1.1](https://developer.nvidia.com/embedded/jetpack) | For best performance, ensure that [power settings](https://docs.nvidia.com/jetson/archives/r34.1/DeveloperGuide/text/SD/PlatformPowerAndPerformance.html) are configured appropriately.
Check out [Tutorial for running Visual SLAM on Jetson](./docs/tutorial-jetson.md). | +| x86_64 | NVIDIA GPU | [Ubuntu 20.04+](https://releases.ubuntu.com/20.04/)
[CUDA 11.8+](https://developer.nvidia.com/cuda-downloads) | ### Docker To simplify development, we strongly recommend leveraging the Isaac ROS Dev Docker images by following [these steps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/docs/dev-env-setup.md). This will streamline your development environment setup with the correct versions of dependencies on both Jetson and x86_64 platforms. -> **Note:** All Isaac ROS Quickstarts, tutorials, and examples have been designed with the Isaac ROS Docker images as a prerequisite. +> **Note**: All Isaac ROS Quickstarts, tutorials, and examples have been designed with the Isaac ROS Docker images as a prerequisite. ## Coordinate Frames @@ -98,40 +114,27 @@ This section describes the coordinate frames that are involved in the `VisualSla ## Quickstart -1. Set up your development environment by following the instructions [here](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/docs/dev-env-setup.md). -2. Clone this repository and its dependencies under `~/workspaces/isaac_ros-dev/src`. +1. Set up your development environment by following the instructions [here](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/docs/dev-env-setup.md). - ```bash - cd ~/workspaces/isaac_ros-dev/src - ``` + > Note: `${ISAAC_ROS_WS}` is defined to point to either `/ssd/workspaces/isaac_ros-dev/` or `~/workspaces/isaac_ros-dev/`. + +2. Clone this repository and its dependencies under `${ISAAC_ROS_WS}/src`. ```bash + cd ${ISAAC_ROS_WS}/src git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam - ``` - - ```bash git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common - ``` - - ```bash git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nitros ``` -3. Pull down a ROS Bag of sample data: +3. \[Terminal 1\] Launch the Docker container ```bash - cd ~/workspaces/isaac_ros-dev/src/isaac_ros_visual_slam && \ - git lfs pull -X "" -I isaac_ros_visual_slam/test/test_cases/rosbags/ + cd ${ISAAC_ROS_WS}/src/isaac_ros_common && \ + ./scripts/run_dev.sh ${ISAAC_ROS_WS} ``` -4. Launch the Docker container using the `run_dev.sh` script: - - ```bash - cd ~/workspaces/isaac_ros-dev/src/isaac_ros_common && \ - ./scripts/run_dev.sh - ``` - -5. Inside the container, build and source the workspace: +4. \[Terminal 1\] Inside the container, build and source the workspace: ```bash cd /workspaces/isaac_ros-dev && \ @@ -139,35 +142,57 @@ This section describes the coordinate frames that are involved in the `VisualSla source install/setup.bash ``` -6. (Optional) Run tests to verify complete and correct installation: + > (Optional) Run tests to verify complete and correct installation: + > + > ```bash + > colcon test --executor sequential + > ``` + + Run the following launch files in the current terminal (Terminal 1): ```bash - colcon test --executor sequential + ros2 launch isaac_ros_visual_slam isaac_ros_visual_slam.launch.py ``` -7. Run the following launch files in the current terminal: +5. \[Terminal 2\] Attach another terminal to the running container for Rviz2 + + Attach another terminal to the running container for RViz2. ```bash - ros2 launch isaac_ros_visual_slam isaac_ros_visual_slam.launch.py + cd ${ISAAC_ROS_WS}/src/isaac_ros_common && \ + ./scripts/run_dev.sh ${ISAAC_ROS_WS} ``` -8. In a second terminal inside the Docker container, prepare rviz to display the output: + From this second terminal, run Rviz2 to display the output: ```bash source /workspaces/isaac_ros-dev/install/setup.bash && \ rviz2 -d src/isaac_ros_visual_slam/isaac_ros_visual_slam/rviz/default.cfg.rviz ``` -9. In an another terminal inside the Docker container, run the following ros bag file to start the demo: + > If you are SSH-ing in from a remote machine, the Rviz2 window should be forwarded to your remote machine. + +6. \[Terminal 3\] Attach the 3rd terminal to start the rosbag + + ```bash + cd ${ISAAC_ROS_WS}/src/isaac_ros_common && \ + ./scripts/run_dev.sh ${ISAAC_ROS_WS} + ``` + + Run the rosbag file to start the demo. ```bash source /workspaces/isaac_ros-dev/install/setup.bash && \ ros2 bag play src/isaac_ros_visual_slam/isaac_ros_visual_slam/test/test_cases/rosbags/small_pol_test/ ``` +7. Result: + Rviz should start displaying the point clouds and poses like below: -
+
+ + > Note: By default, IMU data is ignored. To use IMU data in Isaac ROS Visual Slam, please set `enable_imu` param in your launch file to `True`. See example [below](#tutorial-realsense-imu). ## Next Steps @@ -175,7 +200,8 @@ This section describes the coordinate frames that are involved in the `VisualSla To continue your exploration, check out the following suggested examples: -- [Tutorial with Isaac Sim](docs/tutorial-isaac-sim.md#tutorial-with-isaac-sim) +- [Tutorial with Isaac Sim](docs/tutorial-isaac-sim.md#tutorial-with-isaac-sim) +- [Tutorial for Visual SLAM using a RealSense camera with integrated IMU](docs/tutorial-realsense.md#tutorial-for-visual-slam-using-a-realsense-camera) ### Customize your Dev Environment @@ -269,23 +295,39 @@ For solutions to problems with Isaac ROS, please check [here](https://github.com ### Troubleshooting Suggestions -- If RViz is not showing the poses, check the **Fixed Frame** value. -- If you are seeing `Tracker is lost.` messages frequently, it could be caused by the following issues: - - Fast motion causing the motion blur in the frames. - - Low-lighting conditions. - - The wrong `camerainfo` is being published. -- For better performance: - - Increase the capture framerate from the camera to yield a better tracking result. - - If input images are noisy, you can use the `denoise_input_images` flag in the node. +1. If the target frames in rviz are being updated at a **lower rate** than the input image rate: + 1. Check if the input image frame rate is equal to the value set in the launch file by opening a new terminal and running the command + + ```bash + ros2 topic hz --window 100 + ``` + +2. If RViz is not showing the poses, check the **Fixed Frame** value. +3. If you are seeing `Tracker is lost.` messages frequently, it could be caused by the following issues: + 1. Fast motion causing the motion blur in the frames + 2. Low-lighting conditions. + 3. The wrong `camerainfo` is being published. +4. For better performance: + 1. Increase the capture framerate from the camera to yield a better tracking result. + 2. If input images are noisy, you can use the `denoise_input_images` flag in the node. +5. For RealSense cameras- + 1. The firmware of the RealSense camera is [5.14.0 or newer](https://dev.intelrealsense.com/docs/firmware-releases). Check the RealSense [firmware-update-tool](https://dev.intelrealsense.com/docs/firmware-update-tool) page for details on how to check the version and update the camera firmware. + 2. You are using the [4.51.1 tagged release](https://github.com/IntelRealSense/realsense-ros/releases/tag/4.51.1) for the [realsense-ros package](https://github.com/IntelRealSense/realsense-ros/commit/2a65533ee7431bdc05fe5744798efc7f5713f866). + 3. You are using the parameters set in the [RealSense tutorial launch file](./isaac_ros_visual_slam/launch/isaac_ros_visual_slam_realsense.launch.py) + 4. If the pose estimate frames in RViz is **drifting** from actual real-world poses and you see white dots on nearby objects(refer to the screenshot below), this means that the emitter of the RealSense camera is on and it needs to be [turned off](./isaac_ros_visual_slam/launch/isaac_ros_visual_slam_realsense.launch.py#L35). + 5. Left and right images being published are not compressed or encoded. Isaac ROS Visual Slam expects raw images. + +
## Updates -| Date | Changes | -| ---------- | ------------------------------------------ | -| 2022-10-19 | Updated OSS licensing | -| 2022-08-31 | Update to be compatible with JetPack 5.0.2 | -| 2022-06-30 | Support for ROS2 Humble | -| 2022-03-17 | Documentation update for new features | -| 2022-03-11 | Renamed to isaac_ros_visual_slam | -| 2021-11-15 | Isaac Sim HIL documentation update | -| 2021-10-20 | Initial release | +| Date | Changes | +| ---------- | -------------------------------------------------- | +| 2023-04-05 | Update Elbrus library and performance improvements | +| 2022-10-19 | Updated OSS licensing | +| 2022-08-31 | Update to be compatible with JetPack 5.0.2 | +| 2022-06-30 | Support for ROS 2 Humble | +| 2022-03-17 | Documentation update for new features | +| 2022-03-11 | Renamed to isaac_ros_visual_slam | +| 2021-11-15 | Isaac Sim HIL documentation update | +| 2021-10-20 | Initial release | diff --git a/docs/elbrus-slam.md b/docs/elbrus-slam.md index 32b8f72..3aff0db 100644 --- a/docs/elbrus-slam.md +++ b/docs/elbrus-slam.md @@ -18,6 +18,15 @@ At this moment, a connection is added to the PoseGraph which makes a loop from t The procedure for adding landmarks is designed such that if we do not see a landmark in the place where it was expected, then such landmarks are marked for eventual deletion. This allows you to use Elbrus over a changing terrain. +Along with visual data, Elbrus can use Inertial Measurement Unit (IMU) measurements. It automatically switches to IMU when VO is unable to estimate a pose – for example, when there is dark lighting or long solid surfaces in front of a camera. Using an IMU usually leads to a significant performance improvement in cases of poor visual conditions. + +In case of severe degradation of image input (lights being turned off, dramatic motion blur on a bump while driving, and other possible scenarios), below mentioned motion estimation algorithms will ensure acceptable quality for pose tracking: + +* The IMU readings integrator provides acceptable pose tracking quality for about ~< 1 seconds. + +* In case of IMU failure, the constant velocity integrator continues to provide the last linear and angular velocities reported by Stereo VIO before failure. +This provides acceptable pose tracking quality for ~0.5 seconds. + ## List of Useful Visualizations * `visual_slam/vis/observations_cloud` - Point cloud for 2D Features @@ -28,10 +37,10 @@ The procedure for adding landmarks is designed such that if we do not see a land ## Saving the map -Naturally, we would like to save the stored landmarks and pose graph in a map. We have implemented a ROS2 action to save the map to the disk called `SaveMap`. +Naturally, we would like to save the stored landmarks and pose graph in a map. We have implemented a ROS 2 action to save the map to the disk called `SaveMap`. ## Loading and Localization in the Map -Once the map has been saved to the disk, it can be used later to localize the robot. To load the map into the memory, we have made a ROS2 action called `LoadMapAndLocalize`. It requires a map file path and a prior pose, which is an initial guess of where the robot is in the map. Given the prior pose and current set of camera frames, Elbrus tries to find the pose of the landmarks in the requested map that matches the current set. If the localization is successful, Elbrus will load the map in the memory. Otherwise, it will continue building a new map. +Once the map has been saved to the disk, it can be used later to localize the robot. To load the map into the memory, we have made a ROS 2 action called `LoadMapAndLocalize`. It requires a map file path and a prior pose, which is an initial guess of where the robot is in the map. Given the prior pose and current set of camera frames, Elbrus tries to find the pose of the landmarks in the requested map that matches the current set. If the localization is successful, Elbrus will load the map in the memory. Otherwise, it will continue building a new map. Both `SaveMap` and `LoadMapAndLocalize` can take some time to complete. Hence, they are designed to be asynchronous to avoid interfering with odometry calculations. diff --git a/docs/tutorial-isaac-sim.md b/docs/tutorial-isaac-sim.md index b3516d0..cd80e64 100644 --- a/docs/tutorial-isaac-sim.md +++ b/docs/tutorial-isaac-sim.md @@ -4,7 +4,9 @@ ## Overview -This tutorial walks you through a pipeline to estimate 3D pose of the camera with [Visual SLAM](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam) using images from Isaac Sim. +This tutorial walks you through a graph to estimate 3D pose of the camera with [Visual SLAM](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam) using images from Isaac Sim. + +Last validated with [Isaac Sim 2022.2.1](https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/release_notes.html#id1) ## Tutorial Walkthrough @@ -25,19 +27,19 @@ This tutorial walks you through a pipeline to estimate 3D pose of the camera wit ``` 4. Install and launch Isaac Sim following the steps in the [Isaac ROS Isaac Sim Setup Guide](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/docs/isaac-sim-sil-setup.md) -5. Open up the Isaac ROS Common USD scene (using the "content" window) located at: +5. Open up the Isaac ROS Common USD scene (using the *Content* tab) located at: - `omniverse://localhost/NVIDIA/Assets/Isaac/2022.1/Isaac/Samples/ROS2/Scenario/carter_warehouse_apriltags_worker.usd`. + ```text + http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/2022.2.1/Isaac/Samples/ROS2/Scenario/carter_warehouse_apriltags_worker.usd + ``` And wait for it to load completely. - > **Note:** To use a different server, replace `localhost` with `` -6. Go to the stage tab and select `/World/Carter_ROS/ROS_Cameras/ros2_create_camera_right_info`, then in properties tab -> Compute Node -> Inputs -> stereoOffset X change `0` to `-175.92`. +6. Go to the *Stage* tab and select `/World/Carter_ROS/ROS_Cameras/ros2_create_camera_right_info`, then in *Property* tab *-> OmniGraph Node -> Inputs -> stereoOffset X* change `0` to `-175.92`.
- -7. Enable the right camera for a stereo image pair. Go to the stage tab and select `/World/Carter_ROS/ROS_Cameras/enable_camera_right`, then tick the `Condition` checkbox. +7. Enable the right camera for a stereo image pair. Go to the *Stage* tab and select `/World/Carter_ROS/ROS_Cameras/enable_camera_right`, then tick the *Condition* checkbox.
8. Press **Play** to start publishing data from the Isaac Sim application. -
+
9. In a separate terminal, start `isaac_ros_visual_slam` using the launch files: ```bash @@ -67,17 +69,17 @@ This tutorial walks you through a pipeline to estimate 3D pose of the camera wit ## Saving and using the map -As soon as you start the visual SLAM node, it starts storing the landmarks and the pose graph. You can save them in a map and store the map onto a disk. Make a call to the `SaveMap` ROS2 Action with the following command: +As soon as you start the visual SLAM node, it starts storing the landmarks and the pose graph. You can save them in a map and store the map onto a disk. Make a call to the `SaveMap` ROS 2 Action with the following command: ```bash ros2 action send_goal /visual_slam/save_map isaac_ros_visual_slam_interfaces/action/SaveMap "{map_url: /path/to/save/the/map}" ``` -
+
-
+
Sample run before saving the map
-
+
Now, you will try to load and localize in the previously saved map. First, stop the `visual_slam` node lauched for creating and saving the map, then relaunch it. @@ -88,7 +90,7 @@ ros2 action send_goal /visual_slam/load_map_and_localize isaac_ros_visual_slam_i ```
-
+
Once the above step returns success, you have successfully loaded and localized your robot in the map. If it results in failure, there might be a possibilty that the current landmarks from the approximate start location are not matching with stored landmarks and you need to provide another valid value. @@ -98,7 +100,7 @@ Once the above step returns success, you have successfully loaded and localized
Before Localization
-
+
After localization diff --git a/docs/tutorial-realsense.md b/docs/tutorial-realsense.md new file mode 100644 index 0000000..dfe54c4 --- /dev/null +++ b/docs/tutorial-realsense.md @@ -0,0 +1,267 @@ +# Tutorial for Visual SLAM using a RealSense camera with integrated IMU + +
+ +## Overview + +This tutorial walks you through setting up [Isaac ROS Visual SLAM](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam) with a [Realsense camera](https://www.intel.com/content/www/us/en/architecture-and-technology/realsense-overview.html). + +> **Note**: The [launch file](../isaac_ros_visual_slam/launch/isaac_ros_visual_slam_realsense.launch.py) provided in this tutorial is designed for a RealSense camera with integrated IMU. If you want to run this tutorial with a RealSense camera without an IMU (like RealSense D435), then change `enable_imu` param in the launch file to `False`. + +> **Note**: This tutorial requires a compatible RealSense camera from the list available [here](https://github.com/NVIDIA-ISAAC-ROS/.github/blob/main/profile/realsense-setup.md#camera-compatibility). + +## Tutorial Walkthrough - VSLAM execution + +1. Complete the [RealSense setup tutorial](https://github.com/NVIDIA-ISAAC-ROS/.github/blob/main/profile/realsense-setup.md). + +2. Complete the [Quickstart section](../README.md#quickstart) in the main README. + +3. \[Terminal 1\] Run `realsense-camera` node and `visual_slam` node + + Make sure you have your RealSense camera attached to the system, and then start the Isaac ROS container. + + ```bash + isaac_ros_container + ``` + + > Or if you did not add the command in [step 1-3 of the quickstart section](../README.md#quickstart): + > + > ```bash + > cd ${ISAAC_ROS_WS}/src/isaac_ros_common && \ + > ./scripts/run_dev.sh ${ISAAC_ROS_WS} + > ``` + +4. \[Terminal 1\] Inside the container, build and source the workspace: + + ```bash + cd /workspaces/isaac_ros-dev && \ + colcon build --symlink-install && \ + source install/setup.bash + ``` + +5. \[Terminal 1\] Run the launch file, which launches the example and wait for 5 seconds: + + ```bash + ros2 launch isaac_ros_visual_slam isaac_ros_visual_slam_realsense.launch.py + ``` + +6. \[Terminal 2\] Attach a second terminal to check the operation. + + Attach another terminal to the running container for issuing other ROS2 commands. + + ```bash + isaac_ros_container + ``` + + First check if you can see all the ROS2 topics expected. + + ```bash + ros2 topic list + ``` + + > Output example: + > + > ```bash + > /camera/accel/imu_info + > /camera/accel/metadata + > /camera/accel/sample + > /camera/extrinsics/depth_to_accel + > /camera/extrinsics/depth_to_gyro + > /camera/extrinsics/depth_to_infra1 + > /camera/extrinsics/depth_to_infra2 + > /camera/gyro/imu_info + > /camera/gyro/metadata + > /camera/gyro/sample + > /camera/imu + > /camera/infra1/camera_info + > /camera/infra1/image_rect_raw + > /camera/infra1/image_rect_raw/compressed + > /camera/infra1/image_rect_raw/compressedDepth + > /camera/infra1/image_rect_raw/theora + > /camera/infra1/metadata + > /camera/infra2/camera_info + > /camera/infra2/image_rect_raw + > /camera/infra2/image_rect_raw/compressed + > /camera/infra2/image_rect_raw/compressedDepth + > /camera/infra2/image_rect_raw/theora + > /camera/infra2/metadata + > /parameter_events + > /rosout + > /tf + > /tf_static + > /visual_slam/imu + > /visual_slam/status + > /visual_slam/tracking/odometry + > /visual_slam/tracking/slam_path + > /visual_slam/tracking/vo_path + > /visual_slam/tracking/vo_pose + > /visual_slam/tracking/vo_pose_covariance + > /visual_slam/vis/gravity + > /visual_slam/vis/landmarks_cloud + > /visual_slam/vis/localizer + > /visual_slam/vis/localizer_loop_closure_cloud + > /visual_slam/vis/localizer_map_cloud + > /visual_slam/vis/localizer_observations_cloud + > /visual_slam/vis/loop_closure_cloud + > /visual_slam/vis/observations_cloud + > /visual_slam/vis/pose_graph_edges + > /visual_slam/vis/pose_graph_edges2 + > /visual_slam/vis/pose_graph_nodes + > /visual_slam/vis/velocity + > ``` + + Check the frequency of the `realsense-camera` node's output frequency. + + ```bash + ros2 topic hz /camera/infra1/image_rect_raw --window 20 + ``` + + > Example output: + > + > ```bash + > average rate: 89.714 + > min: 0.011s max: 0.011s std dev: 0.00025s window: 20 + > average rate: 90.139 + > min: 0.010s max: 0.012s std dev: 0.00038s window: 20 + > average rate: 89.955 + > min: 0.011s max: 0.011s std dev: 0.00020s window: 20 + > average rate: 89.761 + > min: 0.009s max: 0.013s std dev: 0.00074s window: 20 + > ``` + > + > `Ctrl` + `c` to stop the output. + + You can also check the frequency of IMU topic. + + ```bash + ros2 topic hz /camera/imu --window 20 + ``` + + > Example output: + > + > ```bash + > average rate: 199.411 + > min: 0.004s max: 0.006s std dev: 0.00022s window: 20 + > average rate: 199.312 + > min: 0.004s max: 0.006s std dev: 0.00053s window: 20 + > average rate: 200.409 + > min: 0.005s max: 0.005s std dev: 0.00007s window: 20 + > average rate: 200.173 + > min: 0.004s max: 0.006s std dev: 0.00028s window: 20 + > ``` + + Lastly, check if you are getting the output from the `visual_slam` node at the same rate as the input. + + ```bash + ros2 topic hz /visual_slam/tracking/odometry --window 20 + ``` + + > Example output + > + > ```bash + > average rate: 58.086 + > min: 0.002s max: 0.107s std dev: 0.03099s window: 20 + > average rate: 62.370 + > min: 0.001s max: 0.109s std dev: 0.03158s window: 20 + > average rate: 90.559 + > min: 0.009s max: 0.013s std dev: 0.00066s window: 20 + > average rate: 85.612 + > min: 0.002s max: 0.100s std dev: 0.02079s window: 20 + > average rate: 90.032 + > min: 0.010s max: 0.013s std dev: 0.00059s window: 20 + > ``` + +## Tutorial Walkthrough - Visualization + +At this point, you have two options for checking the `visual_slam` output. + +- **Live visualization**: Run Rviz2 live while running `realsense-camera` node and `visual_slam` nodes. +- **Offline visualization**: Record rosbag file and check the recorded data offline (possibly on a different machine) + +Running Rviz2 on a remote PC over the network is tricky and is very difficult especially when you have image message topics to subscribe due to added burden on ROS 2 network transport. + +Working on Rviz2 in a X11-forwarded window is also difficult because of the network speed limitation. + +Therefore, if you are running `visual_slam` on Jetson, it is generally recommended **NOT** to evaluate with live visualization (1). + +### Live visualization + +1. \[Terminal 2\] Open Rviz2 from the second terminal: + + ```bash + rviz2 -d src/isaac_ros_visual_slam/isaac_ros_visual_slam/rviz/realsense.cfg.rviz + ``` + + As you move the camera, the position and orientation of the frames should correspond to how the camera moved relative to its starting pose. +
+ +### Offline visualization + +1. \[Terminal 2\] Save rosbag file + + Record the output in your rosbag file (along with the input data for later visual inspection). + + ```bash + export ROSBAG_NAME=courtyard-d435i + ros2 bag record -o ${ROSBAG_NAME} \ + /camera/imu /camera/accel/metadata /camera/gyro/metadata \ + /camera/infra1/camera_info /camera/infra1/image_rect_raw \ + /camera/infra1/metadata \ + /camera/infra2/camera_info /camera/infra2/image_rect_raw \ + /camera/infra2/metadata \ + /tf_static /tf \ + /visual_slam/status \ + /visual_slam/tracking/odometry \ + /visual_slam/tracking/slam_path /visual_slam/tracking/vo_path \ + /visual_slam/tracking/vo_pose /visual_slam/tracking/vo_pose_covariance \ + /visual_slam/vis/landmarks_cloud /visual_slam/vis/loop_closure_cloud \ + /visual_slam/vis/observations_cloud \ + /visual_slam/vis/pose_graph_edges /visual_slam/vis/pose_graph_edges2 \ + /visual_slam/vis/pose_graph_nodes + ros2 bag info ${ROSBAG_NAME} + ``` + + If you plan to run the rosbag on a remote machine (PC) for evaluation, you can send the rosbag file to your remote machine. + + ```bash + export IP_PC=192.168.1.100 + scp -r ${ROSBAG_NAME} ${PC_USER}@${IP_PC}:/home/${PC_USER}/workspaces/isaac_ros-dev/ + ``` + +2. \[Terminal A\] Launch Rviz2 + + > If you are SSH-ing into Jetson from your PC, make sure you enabled X forwarding by adding `-X` option with SSH command. + > + > ```bash + > ssh -X ${USERNAME_ON_JETSON}@${IP_JETSON} + > ``` + + Launch the Isaac ROS container. + + ```bash + isaac_ros_container + ``` + + Run Rviz with a configuration file for visualizing set of messages from Visual SLAM node. + + ```bash + cd /workspaces/isaac_ros-dev + rviz2 -d src/isaac_ros_visual_slam/isaac_ros_visual_slam/rviz/vslam_keepall.cfg.rviz + ``` + +3. \[Terminal B\] Playback the recorded rosbag + + Attach another terminal to the running container. + + ```bash + isaac_ros_container + ``` + + Play the recorded rosbag file. + + ```bash + ros2 bag play ${ROSBAG_NAME} + ``` + + RViz should start showing visualization like the following. +
diff --git a/isaac_ros_visual_slam/CMakeLists.txt b/isaac_ros_visual_slam/CMakeLists.txt index 9e0335d..a1d30e2 100644 --- a/isaac_ros_visual_slam/CMakeLists.txt +++ b/isaac_ros_visual_slam/CMakeLists.txt @@ -15,33 +15,20 @@ # # SPDX-License-Identifier: Apache-2.0 -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.23.2) project(isaac_ros_visual_slam LANGUAGES C CXX) -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# Default to Release build -if(NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") - set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) -endif() -message( STATUS "CMAKE_BUILD_TYPE: ${CMAKE_BUILD_TYPE}" ) - -execute_process( - COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCHITECTURE) +execute_process(COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCHITECTURE) message( STATUS "Architecture: ${ARCHITECTURE}" ) -# find dependencies find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -# Eigen +# Dependencies find_package(Eigen3 REQUIRED) find_package(Threads REQUIRED) include_directories(${EIGEN3_INCLUDE_DIR}) @@ -59,9 +46,9 @@ endforeach() include_directories(${ELBRUS}/include) add_library(elbrus SHARED IMPORTED) if( ${ARCHITECTURE} STREQUAL "x86_64" ) - set_property(TARGET elbrus PROPERTY IMPORTED_LOCATION ${ELBRUS}/lib_x86_64/libelbrus.so) + set_property(TARGET elbrus PROPERTY IMPORTED_LOCATION ${ELBRUS}/lib_x86_64_cuda_11_8/libelbrus.so) elseif( ${ARCHITECTURE} STREQUAL "aarch64" ) - set_property(TARGET elbrus PROPERTY IMPORTED_LOCATION ${ELBRUS}/lib_aarch64_jetpack50/libelbrus.so) + set_property(TARGET elbrus PROPERTY IMPORTED_LOCATION ${ELBRUS}/lib_aarch64_jetpack51/libelbrus.so) endif() # visual_slam_node @@ -77,9 +64,6 @@ ament_auto_add_library( src/impl/viz_helper.cpp src/impl/qos.cpp ) -target_compile_definitions(visual_slam_node - PRIVATE "COMPOSITION_BUILDING_DLL" -) target_link_libraries(visual_slam_node elbrus) rclcpp_components_register_nodes(visual_slam_node "isaac_ros::visual_slam::VisualSlamNode") set(node_plugins "${node_plugins}isaac_ros::visual_slam::VisualSlamNode;$\n") @@ -90,17 +74,11 @@ ament_auto_add_executable(${PROJECT_NAME} ) target_link_libraries(${PROJECT_NAME} visual_slam_node) -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - # Install Elbrus if( ${ARCHITECTURE} STREQUAL "x86_64" ) - install(DIRECTORY ${ELBRUS}/lib_x86_64/ DESTINATION share/elbrus) + install(DIRECTORY ${ELBRUS}/lib_x86_64_cuda_11_8/ DESTINATION share/elbrus) elseif( ${ARCHITECTURE} STREQUAL "aarch64" ) - install(DIRECTORY ${ELBRUS}/lib_aarch64_jetpack50/ DESTINATION share/elbrus) + install(DIRECTORY ${ELBRUS}/lib_aarch64_jetpack51/ DESTINATION share/elbrus) endif() set_target_properties(visual_slam_node PROPERTIES BUILD_WITH_INSTALL_RPATH TRUE) set_target_properties(visual_slam_node PROPERTIES INSTALL_RPATH "$ORIGIN/../share/elbrus") @@ -113,4 +91,4 @@ if(BUILD_TESTING) add_launch_test(test/isaac_ros_visual_slam_pol.py) endif() -ament_auto_package(INSTALL_TO_SHARE launch) +ament_auto_package(INSTALL_TO_SHARE launch params) diff --git a/isaac_ros_visual_slam/launch/isaac_ros_visual_slam_realsense.launch.py b/isaac_ros_visual_slam/launch/isaac_ros_visual_slam_realsense.launch.py index 56ca954..511037f 100644 --- a/isaac_ros_visual_slam/launch/isaac_ros_visual_slam_realsense.launch.py +++ b/isaac_ros_visual_slam/launch/isaac_ros_visual_slam_realsense.launch.py @@ -33,7 +33,10 @@ def generate_launch_description(): 'enable_color': False, 'enable_depth': False, 'depth_module.emitter_enabled': 0, - 'depth_module.profile': '640x360x90' + 'depth_module.profile': '640x360x90', + 'enable_gyro': True, + 'enable_accel': True, + 'unite_imu_method': 2 }] ) @@ -52,12 +55,15 @@ def generate_launch_description(): 'enable_observations_view': True, 'map_frame': 'map', 'odom_frame': 'odom', - 'base_frame': 'camera_link' + 'base_frame': 'camera_link', + 'input_imu_frame': 'camera_gyro_optical_frame', + 'enable_imu': True }], remappings=[('stereo_camera/left/image', 'camera/infra1/image_rect_raw'), ('stereo_camera/left/camera_info', 'camera/infra1/camera_info'), ('stereo_camera/right/image', 'camera/infra2/image_rect_raw'), - ('stereo_camera/right/camera_info', 'camera/infra2/camera_info')] + ('stereo_camera/right/camera_info', 'camera/infra2/camera_info'), + ('visual_slam/imu', 'camera/imu')] ) visual_slam_launch_container = ComposableNodeContainer( diff --git a/isaac_ros_visual_slam/package.xml b/isaac_ros_visual_slam/package.xml index 25109a5..dad37ba 100644 --- a/isaac_ros_visual_slam/package.xml +++ b/isaac_ros_visual_slam/package.xml @@ -21,13 +21,13 @@ isaac_ros_visual_slam - 0.20.0 + 0.30.0 Visual SLAM Package Swapnesh Wani Apache-2.0 https://developer.nvidia.com/isaac-ros-gems/ Swapnesh Wani - + ament_cmake_auto rclcpp @@ -43,6 +43,8 @@ isaac_ros_nitros isaac_ros_visual_slam_interfaces + isaac_ros_common + ament_lint_auto ament_lint_common isaac_ros_test diff --git a/isaac_ros_visual_slam/params/ekf_dual_nvslam.yaml b/isaac_ros_visual_slam/params/ekf_dual_nvslam.yaml new file mode 100644 index 0000000..dce996a --- /dev/null +++ b/isaac_ros_visual_slam/params/ekf_dual_nvslam.yaml @@ -0,0 +1,277 @@ +### ekf config file ### +ekf_filter_node: + ros__parameters: +# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin +# computation until it receives at least one message from one of the inputs. It will then run continuously at the +# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified. + frequency: 30.0 + +# The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict +# cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the +# filter will generate new output. Defaults to 1 / frequency if not specified. + sensor_timeout: 0.1 + +# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is +# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar +# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected +# by, for example, an IMU. Defaults to false if unspecified. + two_d_mode: true + +# Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for +# future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if +# unspecified. + transform_time_offset: 0.0 + +# Use this parameter to provide specify how long the tf listener should wait for a transform to become available. +# Defaults to 0.0 if unspecified. + transform_timeout: 0.0 + +# If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is +# unhappy with any settings or data. + print_diagnostics: true + +# Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by +# debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious +# effects on the performance of the node. Defaults to false if unspecified. + debug: false + +# Defaults to "robot_localization_debug.txt" if unspecified. Please specify the full path. + debug_out_file: /path/to/debug/file.txt + +# Whether we'll allow old measurements to cause a re-publication of the updated state + permit_corrected_publication: false + +# Whether to publish the acceleration state. Defaults to false if unspecified. + publish_acceleration: false + +# Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified. + publish_tf: true + +# REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and +# earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames. +# The robot's position in the odom frame will drift over time, but is accurate in the short term and should be +# continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom +# frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your +# robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based +# localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame. +# ekf_localization_node and ukf_localization_node are not concerned with the earth frame. +# Here is how to use the following settings: +# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system. +# 1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of +# odom_frame. +# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set +# "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes. +# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates +# from landmark observations) then: +# 3a. Set your "world_frame" to your map_frame value +# 3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state +# estimation node from robot_localization! However, that instance should *not* fuse the global data. + map_frame: map # Defaults to "map" if unspecified + odom_frame: odom # Defaults to "odom" if unspecified + base_link_frame: base_link # Defaults to "base_link" if unspecified + world_frame: odom # Defaults to the value of odom_frame if unspecified + +# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, +# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, +# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0, +# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no +# default values, and must be specified. + odom0: /visual_slam_back/visual_slam/tracking/odometry + +# Each sensor reading updates some or all of the filter's state. These options give you greater control over which +# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only +# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the +# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types +# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message +# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false +# if unspecified, effectively making this parameter required for each sensor. + odom0_config: [true, true, true, + true, true, true, + false, false, false, + false, false, false, + false, false, false] + +# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase +# the size of the subscription queue so that more measurements are fused. + odom0_queue_size: 2 + +# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result +# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's +# algorithm. + odom0_nodelay: false + +# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- +# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they +# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also +# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't +# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose +# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then +# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true +# for twist measurements has no effect. + odom0_differential: true + +# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" +# for all future measurements. While you can achieve the same effect with the differential paremeter, the key +# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before +# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. + odom0_relative: true + +# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to +# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to +# numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not +# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation. +# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying +# the thresholds. + odom0_pose_rejection_threshold: 5.0 + odom0_twist_rejection_threshold: 1.0 + + + + + + + + +# The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry, +# geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped, +# sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0, +# odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no +# default values, and must be specified. + odom1: /visual_slam_front/visual_slam/tracking/odometry + +# Each sensor reading updates some or all of the filter's state. These options give you greater control over which +# values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only +# want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the +# values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types +# do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message +# has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false +# if unspecified, effectively making this parameter required for each sensor. + odom1_config: [true, true, true, + true, true, true, + false, false, false, + false, false, false, + false, false, false] + +# If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase +# the size of the subscription queue so that more measurements are fused. + odom1_queue_size: 2 + +# [ADVANCED] Large messages in ROS can exhibit strange behavior when they arrive at a high frequency. This is a result +# of Nagle's algorithm. This option tells the ROS subscriber to use the tcpNoDelay option, which disables Nagle's +# algorithm. + odom1_nodelay: false + +# [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under- +# report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they +# arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also +# measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't +# always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose +# data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then +# integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true +# for twist measurements has no effect. + odom1_differential: true + +# [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point" +# for all future measurements. While you can achieve the same effect with the differential paremeter, the key +# difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before +# integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true. + odom1_relative: true + +# [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to +# control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to +# numeric_limits::max() if unspecified. It is strongly recommended that these parameters be removed if not +# required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation. +# For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying +# the thresholds. + odom1_pose_rejection_threshold: 5.0 + odom1_twist_rejection_threshold: 1.0 + + + + + + + + + + + + +# [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set +# this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. + # imu0_remove_gravitational_acceleration: true + +# [ADVANCED] The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no +# acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During +# correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be +# problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When +# this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially +# noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance +# for the velocity variable in question, or decrease the variance of the variable in question in the measurement +# itself. In addition, users can also take advantage of the control command being issued to the robot at the time we +# make the prediction. If control is used, it will get converted into an acceleration term, which will be used during +# predicition. Note that if an acceleration measurement for the variable in question is available from one of the +# inputs, the control term will be ignored. +# Whether or not we use the control input during predicition. Defaults to false. + use_control: true +# Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to +# false. + stamped_control: false +# The last issued control command will be used in prediction for this period. Defaults to 0.2. + control_timeout: 0.2 +# Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. + control_config: [true, true, false, false, false, true] +# Places limits on how large the acceleration term will be. Should match your robot's kinematics. + acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] +# Acceleration and deceleration limits are not always the same for robots. + deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] +# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these +# gains + acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] +# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these +# gains + deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] +# [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is +# exposed as a configuration parameter. This matrix represents the noise we add to the total error after each +# prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be. +# However, if users find that a given variable is slow to converge, one approach is to increase the +# process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error +# to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are +# ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if +# unspecified. + process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015] +# [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal +# value (variance) to a large value will result in rapid convergence for initial measurements of the variable in +# question. Users should take care not to use large values for variables that will not be measured directly. The values +# are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below +#if unspecified. + initial_estimate_covariance: [1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-9] + diff --git a/isaac_ros_visual_slam/rviz/dual_realsense.cfg.rviz b/isaac_ros_visual_slam/rviz/dual_realsense.cfg.rviz new file mode 100644 index 0000000..f664154 --- /dev/null +++ b/isaac_ros_visual_slam/rviz/dual_realsense.cfg.rviz @@ -0,0 +1,298 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1 + - /TF1/Frames1 + - /TF1/Tree1 + Splitter Ratio: 0.5 + Tree Height: 128 + - 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: "" +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: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_link: + Value: false + base_link_vslam_1: + Value: false + base_link_vslam_2: + Value: false + base_link_vslam_back: + Value: true + base_link_vslam_front: + Value: true + camera_back_accel_frame: + Value: false + camera_back_accel_optical_frame: + Value: false + camera_back_aligned_depth_to_infra1_frame: + Value: false + camera_back_gyro_frame: + Value: false + camera_back_gyro_optical_frame: + Value: false + camera_back_infra1_frame: + Value: false + camera_back_infra1_optical_frame: + Value: false + camera_back_infra2_frame: + Value: false + camera_back_infra2_optical_frame: + Value: false + camera_back_link: + Value: false + camera_front_accel_frame: + Value: false + camera_front_accel_optical_frame: + Value: false + camera_front_aligned_depth_to_infra1_frame: + Value: false + camera_front_gyro_frame: + Value: false + camera_front_gyro_optical_frame: + Value: false + camera_front_infra1_frame: + Value: false + camera_front_infra1_optical_frame: + Value: false + camera_front_infra2_frame: + Value: false + camera_front_infra2_optical_frame: + Value: false + camera_front_link: + Value: false + odom: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + base_link_vslam_1: + {} + odom: + base_link: + {} + base_link_vslam_2: + camera_front_link: + camera_front_accel_frame: + camera_front_accel_optical_frame: + {} + camera_front_aligned_depth_to_infra1_frame: + camera_front_infra1_optical_frame: + {} + camera_front_gyro_frame: + camera_front_gyro_optical_frame: + {} + camera_front_infra1_frame: + {} + camera_front_infra2_frame: + camera_front_infra2_optical_frame: + {} + base_link_vslam_back: + camera_back_link: + camera_back_accel_frame: + camera_back_accel_optical_frame: + {} + camera_back_aligned_depth_to_infra1_frame: + camera_back_infra1_optical_frame: + {} + camera_back_gyro_frame: + camera_back_gyro_optical_frame: + {} + camera_back_infra1_frame: + {} + camera_back_infra2_frame: + camera_back_infra2_optical_frame: + {} + base_link_vslam_front: + {} + Update Interval: 0 + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera_front/infra1/image_rect_raw + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera_front/infra2/image_rect_raw + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera_back/infra1/image_rect_raw + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera_back/infra2/image_rect_raw + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: odom + 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: 5.033440589904785 + 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: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6253983974456787 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.9904189109802246 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1136 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd000000040000000000000216000003d6fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003b00000109000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000003b000000e30000002800fffffffb0000000a0049006d0061006700650100000124000000e10000002800fffffffb0000000a0049006d006100670065010000020b000000ed0000002800fffffffb0000000a0049006d00610067006501000002fe000001130000002800ffffff000000010000010f000003d6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003d6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000051c000003d600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 72 + Y: 27 diff --git a/isaac_ros_visual_slam/rviz/realsense.cfg.rviz b/isaac_ros_visual_slam/rviz/realsense.cfg.rviz new file mode 100644 index 0000000..c0bf00f --- /dev/null +++ b/isaac_ros_visual_slam/rviz/realsense.cfg.rviz @@ -0,0 +1,539 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 172 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /TF1/Tree1 + - /Loop closure1/Topic1 + - /Localizer1/Localizer Map1 + - /Odometry1/Shape1 + Splitter Ratio: 0.6470588445663452 + Tree Height: 271 + - 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 + - /Current View1/Focal Point1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + camera_accel_frame: + Value: true + camera_accel_optical_frame: + Value: true + camera_aligned_depth_to_infra1_frame: + Value: true + camera_gyro_frame: + Value: true + camera_gyro_optical_frame: + Value: true + camera_infra1_frame: + Value: true + camera_infra1_optical_frame: + Value: true + camera_infra2_frame: + Value: true + camera_infra2_optical_frame: + Value: true + camera_link: + Value: true + map: + Value: true + odom: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + odom: + camera_link: + camera_accel_frame: + camera_accel_optical_frame: + {} + camera_aligned_depth_to_infra1_frame: + camera_infra1_optical_frame: + {} + camera_gyro_frame: + camera_gyro_optical_frame: + {} + camera_infra1_frame: + {} + camera_infra2_frame: + camera_infra2_optical_frame: + {} + Update Interval: 0 + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Gravity + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/gravity + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Landmarks + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/landmarks_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Observations + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.019999999552965164 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/observations_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Loop closure + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.029999999329447746 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/loop_closure_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 25; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: VO Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/tracking/vo_path + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: SLAM Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/tracking/slam_path + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.10000000149011612 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: PG Nodes + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/pose_graph_nodes + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: PG Edges2 + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/pose_graph_edges2 + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: PG Edges + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/pose_graph_edges + Value: true + Enabled: true + Name: Pose Graph + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Localizer + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/localizer + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Localizer Map + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/localizer_map_cloud + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: true + Name: Localizer + - Angle Tolerance: 0.009999999776482582 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: false + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: false + Value: true + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.009999999776482582 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/tracking/odometry + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Velocity + Namespaces: + angular velocity: true + linear velocity: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/velocity + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/infra1/image_rect_raw + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/infra2/image_rect_raw + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 6.531785011291504 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.814717411994934 + Y: 0.6073047518730164 + Z: -0.6894067525863647 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5353977084159851 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.7303991317749023 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1136 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000041afc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000ea0000036b000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000003b000002050000002800fffffffb0000000a0049006d00610067006501000002460000020f0000002800fffffffb0000000a0049006d00610067006501000001fc00000259000000000000000000000001000001b40000041afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000041a000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000738000000a9fc0100000003fb0000000a0049006d0061006700650100000000000003b40000000000000000fb0000000a0049006d00610067006501000003ba0000037e0000000000000000fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005dc0000041a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 72 + Y: 27 diff --git a/isaac_ros_visual_slam/rviz/vslam_keepall.cfg.rviz b/isaac_ros_visual_slam/rviz/vslam_keepall.cfg.rviz new file mode 100644 index 0000000..5738f13 --- /dev/null +++ b/isaac_ros_visual_slam/rviz/vslam_keepall.cfg.rviz @@ -0,0 +1,375 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /vo_path1 + - /Odometry1 + - /Odometry1/Topic1 + - /Odometry1/Shape1 + - /Odometry1/Covariance1 + - /Odometry1/Covariance1/Position1 + - /Odometry1/Covariance1/Orientation1 + - /vo_pose1 + - /vo_pose1/Topic1 + Splitter Ratio: 0.5 + Tree Height: 531 + - 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: Loop-closures +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: + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: false + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.30000001192092896 + Name: slam_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/tracking/slam_path + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 88; 252 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.5 + Name: vo_path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/tracking/vo_path + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Landmarks + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/landmarks_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Observations + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.30000001192092896 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/observations_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Loop-closures + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.30000001192092896 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /visual_slam/vis/loop_closure_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 5000 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 25; 255; 0 + Head Length: 0.5 + Head Radius: 0.10000000149011612 + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.5 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep All + Reliability Policy: Reliable + Value: /visual_slam/tracking/odometry + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/infra1/image_rect_raw + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/Pose + Color: 144; 48; 252 + Enabled: true + Head Length: 1 + Head Radius: 1 + Name: vo_pose + Shaft Length: 1 + Shaft Radius: 0.5 + Shape: Arrow + Topic: + Depth: 10 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep All + Reliability Policy: Reliable + Value: /visual_slam/tracking/vo_pose + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 91.45012664794922 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 25.506731033325195 + Y: 26.043254852294922 + Z: -34.239601135253906 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.9253974556922913 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.14668607711792 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1163 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000025b000003edfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000250000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000163000000860000000000000000fb0000000a0049006d00610067006501000001ef0000009f0000000000000000fb0000000a0049006d0061006700650100000293000001970000002800ffffff000000010000010f000003edfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003ed000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000051f000003ed00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1920 + X: 1920 + Y: 0 diff --git a/isaac_ros_visual_slam/src/visual_slam_node.cpp b/isaac_ros_visual_slam/src/visual_slam_node.cpp index b003578..f5320a4 100644 --- a/isaac_ros_visual_slam/src/visual_slam_node.cpp +++ b/isaac_ros_visual_slam/src/visual_slam_node.cpp @@ -112,44 +112,44 @@ VisualSlamNode::VisualSlamNode(rclcpp::NodeOptions options) // Visualizators for odometry vis_observations_pub_( create_publisher( - "visual_slam/vis/observations_cloud", rclcpp::QoS(1))), + "visual_slam/vis/observations_cloud", rclcpp::QoS(10))), vis_gravity_pub_( create_publisher( - "visual_slam/vis/gravity", rclcpp::QoS(1))), + "visual_slam/vis/gravity", rclcpp::QoS(10))), vis_vo_velocity_pub_( create_publisher( - "visual_slam/vis/velocity", rclcpp::QoS(1))), + "visual_slam/vis/velocity", rclcpp::QoS(10))), // Visualizators for map and "loop closure" vis_landmarks_pub_( create_publisher( - "visual_slam/vis/landmarks_cloud", rclcpp::QoS(1))), + "visual_slam/vis/landmarks_cloud", rclcpp::QoS(10))), vis_loop_closure_pub_( create_publisher( - "visual_slam/vis/loop_closure_cloud", rclcpp::QoS(1))), + "visual_slam/vis/loop_closure_cloud", rclcpp::QoS(10))), vis_posegraph_nodes_pub_( create_publisher( - "visual_slam/vis/pose_graph_nodes", rclcpp::QoS(1))), + "visual_slam/vis/pose_graph_nodes", rclcpp::QoS(10))), vis_posegraph_edges_pub_( create_publisher( - "visual_slam/vis/pose_graph_edges", rclcpp::QoS(1))), + "visual_slam/vis/pose_graph_edges", rclcpp::QoS(10))), vis_posegraph_edges2_pub_( create_publisher( - "visual_slam/vis/pose_graph_edges2", rclcpp::QoS(1))), + "visual_slam/vis/pose_graph_edges2", rclcpp::QoS(10))), // Visualizators for localization vis_localizer_pub_( create_publisher( - "visual_slam/vis/localizer", rclcpp::QoS(1))), + "visual_slam/vis/localizer", rclcpp::QoS(10))), vis_localizer_landmarks_pub_( create_publisher( - "visual_slam/vis/localizer_map_cloud", rclcpp::QoS(1))), + "visual_slam/vis/localizer_map_cloud", rclcpp::QoS(10))), vis_localizer_observations_pub_( create_publisher( - "visual_slam/vis/localizer_observations_cloud", rclcpp::QoS(1))), + "visual_slam/vis/localizer_observations_cloud", rclcpp::QoS(10))), vis_localizer_loop_closure_pub_( create_publisher( - "visual_slam/vis/localizer_loop_closure_cloud", rclcpp::QoS(1))), + "visual_slam/vis/localizer_loop_closure_cloud", rclcpp::QoS(10))), // Slam Services reset_srv_( @@ -172,7 +172,7 @@ VisualSlamNode::VisualSlamNode(rclcpp::NodeOptions options) ELBRUS_GetVersion(&elbrus_major, &elbrus_minor); RCLCPP_INFO(get_logger(), "Elbrus version: %d.%d", elbrus_major, elbrus_minor); // VisualSlamNode is desined for this elbrus sdk version: - int32_t exp_elbrus_major = 10, exp_elbrus_minor = 0; + int32_t exp_elbrus_major = 10, exp_elbrus_minor = 5; if (elbrus_major != exp_elbrus_major || elbrus_minor != exp_elbrus_minor) { RCLCPP_ERROR( get_logger(), "VisualSlamNode is designed to work with Elbrus SDK v%d.%d", @@ -232,17 +232,18 @@ void VisualSlamNode::ReadImuData(const sensor_msgs::msg::Imu::ConstSharedPtr msg exit(EXIT_FAILURE); } - // If elbrus handle is not initialized, then return. - if (impl_->IsInitialized() && enable_imu_) { - const auto measurement = impl_->ToElbrusImuMeasurement(msg_imu); - - rclcpp::Time timestamp(msg_imu->header.stamp.sec, msg_imu->header.stamp.nanosec); - const int64_t acqtime_ns = static_cast(timestamp.nanoseconds()); + if (!impl_->IsInitialized() || !enable_imu_) { + return; + } + const auto measurement = impl_->ToElbrusImuMeasurement(msg_imu); + const rclcpp::Time timestamp(msg_imu->header.stamp.sec, msg_imu->header.stamp.nanosec); + const int64_t acqtime_ns = static_cast(timestamp.nanoseconds()); - if (!ELBRUS_RegisterImuMeasurement(impl_->elbrus_handle, acqtime_ns, &measurement)) { - RCLCPP_WARN(this->get_logger(), "ELBRUS has failed to register an IMU measurement"); - } - } else {return;} + const ELBRUS_Status status = ELBRUS_RegisterImuMeasurement( + impl_->elbrus_handle, acqtime_ns, &measurement); + if (status != ELBRUS_SUCCESS) { + RCLCPP_WARN(this->get_logger(), "ELBRUS has failed to register an IMU measurement"); + } } void VisualSlamNode::CallbackReset( @@ -612,14 +613,16 @@ void VisualSlamNode::TrackCameraPose( impl_->base_link_pose_elbrus, start_elbrus_pose_rectified_ebrus)}; // Use start odom pose - odom_pose_rectified_left = impl_->start_odom_pose * odom_pose_rectified_left; + odom_pose_rectified_left = ChangeBasis( + impl_->start_odom_pose, odom_pose_rectified_left); // Frames hierarchy: // map - odom - base_link - ... - camera const tf2::Transform map_pose_odom = odom_pose_rectified_left * odom_pose_smooth_left.inverse(); - const tf2::Transform odom_pose_base_frame = odom_pose_smooth_left * - impl_->left_pose_base_link; + + const tf2::Transform odom_pose_base_frame = ChangeBasis( + impl_->start_odom_pose, odom_pose_smooth_left); rclcpp::Time timestamp_output = msg_left_img->header.stamp; if (override_publishing_stamp_) { @@ -648,6 +651,12 @@ void VisualSlamNode::TrackCameraPose( impl_->pose_cache.Add(timestamp.nanoseconds(), odom_pose_base_frame); geometry_msgs::msg::Twist velocity; + // Extract timestamp from left image and create headers for + // slam and vo poses + std_msgs::msg::Header header_map = msg_left_img->header; + header_map.frame_id = map_frame_; + std_msgs::msg::Header header_odom = msg_left_img->header; + header_odom.frame_id = odom_frame_; impl_->pose_cache.GetVelocity( velocity.linear.x, velocity.linear.y, velocity.linear.z, @@ -658,11 +667,9 @@ void VisualSlamNode::TrackCameraPose( { // Pose Tracking - std_msgs::msg::Header header = msg_left_img->header; - header.frame_id = map_frame_; geometry_msgs::msg::Pose vo_pose; - tf2::toMsg(odom_pose_smooth_left, vo_pose); + tf2::toMsg(odom_pose_base_frame, vo_pose); geometry_msgs::msg::Pose slam_pose; tf2::toMsg(odom_pose_rectified_left, slam_pose); @@ -670,14 +677,14 @@ void VisualSlamNode::TrackCameraPose( if (HasSubscribers(*this, tracking_vo_pose_pub_)) { // Tracking_vo_pose_pub_ auto pose_only = std::make_unique(); - pose_only->header = header; + pose_only->header = header_odom; pose_only->pose = vo_pose; tracking_vo_pose_pub_->publish(std::move(pose_only)); } if (HasSubscribers(*this, tracking_vo_pose_covariance_pub_)) { // Tracking_vo_covariance_pub_ auto pose_n_cov = std::make_unique(); - pose_n_cov->header = header; + pose_n_cov->header = header_odom; pose_n_cov->pose.pose = vo_pose; for (size_t i = 0; i < 6 * 6; i++) { pose_n_cov->pose.covariance[i] = static_cast(pose_estimate.covariance[i]); @@ -686,8 +693,7 @@ void VisualSlamNode::TrackCameraPose( } if (HasSubscribers(*this, tracking_odometry_pub_)) { nav_msgs::msg::Odometry odom; - odom.header = header; - odom.header.frame_id = odom_frame_; + odom.header = header_odom; odom.child_frame_id = base_frame_; odom.pose.pose = vo_pose; @@ -725,24 +731,24 @@ void VisualSlamNode::TrackCameraPose( if (HasSubscribers(*this, tracking_vo_path_pub_)) { // Tracking_vo_path_pub_ geometry_msgs::msg::PoseStamped pose_stamped; - pose_stamped.header = header; + pose_stamped.header = header_odom; pose_stamped.pose = vo_pose; impl_->vo_path.add(pose_stamped); auto path_msg = std::make_unique(); - path_msg->header = header; + path_msg->header = header_odom; path_msg->poses = impl_->vo_path.getData(); tracking_vo_path_pub_->publish(std::move(path_msg)); } if (HasSubscribers(*this, tracking_slam_path_pub_)) { // Tracking_slam_path_pub_ geometry_msgs::msg::PoseStamped pose_stamped; - pose_stamped.header = header; + pose_stamped.header = header_map; pose_stamped.pose = slam_pose; impl_->slam_path.add(pose_stamped); auto path_msg = std::make_unique(); - path_msg->header = header; + path_msg->header = header_map; path_msg->poses = impl_->slam_path.getData(); tracking_slam_path_pub_->publish(std::move(path_msg)); } @@ -772,7 +778,7 @@ void VisualSlamNode::TrackCameraPose( } isaac_ros_visual_slam_interfaces::msg::VisualSlamStatus visual_slam_status_msg; - visual_slam_status_msg.header = msg_left_img->header; + visual_slam_status_msg.header = header_map; visual_slam_status_msg.vo_state = pose_estimate.vo_state; visual_slam_status_msg.integrator_state = pose_estimate.integrator_state; visual_slam_status_msg.node_callback_execution_time = stopwatch.Seconds(); diff --git a/isaac_ros_visual_slam_interfaces/CMakeLists.txt b/isaac_ros_visual_slam_interfaces/CMakeLists.txt index 8434790..3c33e20 100644 --- a/isaac_ros_visual_slam_interfaces/CMakeLists.txt +++ b/isaac_ros_visual_slam_interfaces/CMakeLists.txt @@ -15,24 +15,13 @@ # # SPDX-License-Identifier: Apache-2.0 -cmake_minimum_required(VERSION 3.5) -project(isaac_ros_visual_slam_interfaces LANGUAGES C CXX) - -# Default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() +cmake_minimum_required(VERSION 3.23.2) +project(isaac_ros_visual_slam_interfaces LANGUAGES C CXX) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# Default to Release build -if(NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") - set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) -endif() -message( STATUS "CMAKE_BUILD_TYPE: ${CMAKE_BUILD_TYPE}" ) - find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() diff --git a/isaac_ros_visual_slam_interfaces/package.xml b/isaac_ros_visual_slam_interfaces/package.xml index a131127..b8d66d6 100644 --- a/isaac_ros_visual_slam_interfaces/package.xml +++ b/isaac_ros_visual_slam_interfaces/package.xml @@ -21,7 +21,7 @@ isaac_ros_visual_slam_interfaces - 0.20.0 + 0.30.0 Interfaces for Isaac ROS Visual SLAM Swapnesh Wani Apache-2.0 @@ -34,6 +34,7 @@ rosidl_default_runtime geometry_msgs + isaac_ros_common ament_lint_auto ament_lint_common diff --git a/resources/Isaac_sim_enable_stereo.png b/resources/Isaac_sim_enable_stereo.png index 827f8da..69c2126 100644 --- a/resources/Isaac_sim_enable_stereo.png +++ b/resources/Isaac_sim_enable_stereo.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:602b513e8a85d563f0f87b312677de2c92d04e9c9b8800d31f71d22e6a189c84 -size 245916 +oid sha256:8c9ba1412d085b83a42833d12765e6ea331000047bc48849359858c5e9167beb +size 109035 diff --git a/resources/Isaac_sim_play.png b/resources/Isaac_sim_play.png new file mode 100644 index 0000000..7208c9e --- /dev/null +++ b/resources/Isaac_sim_play.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e5c0d786681c57ab859790262f19157b22a18a1f4f0bd254fa7f65df12fbe516 +size 784360 diff --git a/resources/Isaac_sim_set_stereo_offset.png b/resources/Isaac_sim_set_stereo_offset.png index 653e042..f6ce4c2 100644 --- a/resources/Isaac_sim_set_stereo_offset.png +++ b/resources/Isaac_sim_set_stereo_offset.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:181371ea36dcb597d3a5df938191954742e6d18812e9b0ad4cb8f25d985ff8e0 -size 113557 +oid sha256:bb562c0a01984e1115ae37811b749b14e2bb1a4429a5e3e2043157ef52d9aa36 +size 112275 diff --git a/resources/Isaac_sim_visual_slam.png b/resources/Isaac_sim_visual_slam.png deleted file mode 100644 index eb2fcbd..0000000 --- a/resources/Isaac_sim_visual_slam.png +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c82817945a9e9617dc082ea389b5374050dce911ef5a104ee2c7aef577006547 -size 3756879 diff --git a/resources/RViz_0217-cube_vslam-keepall.gif b/resources/RViz_0217-cube_vslam-keepall.gif new file mode 100644 index 0000000..93ba532 --- /dev/null +++ b/resources/RViz_0217-cube_vslam-keepall.gif @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ea9005730da4892ac76e72f185a2b27ea69c0b9a541ff3d911d291196c42c66e +size 1336375 diff --git a/resources/RViz_0217-cube_vslam-keepall.png b/resources/RViz_0217-cube_vslam-keepall.png new file mode 100644 index 0000000..a301e3d --- /dev/null +++ b/resources/RViz_0217-cube_vslam-keepall.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:aafc3519fa1c0397b1ff37f10004286df94dae360e4ffa9c0252d2a975651d44 +size 1162204 diff --git a/resources/VSLAM-and-RViz2-on-Jetson_X11forward-to-PC.png b/resources/VSLAM-and-RViz2-on-Jetson_X11forward-to-PC.png new file mode 100644 index 0000000..780b617 --- /dev/null +++ b/resources/VSLAM-and-RViz2-on-Jetson_X11forward-to-PC.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1bbf27ade84e424ae5b9b0d53d4b5f0615e775d45041ba594035cfbbe8ec0b53 +size 557222 diff --git a/resources/back_cam_transform.png b/resources/back_cam_transform.png new file mode 100644 index 0000000..9dfb4e5 --- /dev/null +++ b/resources/back_cam_transform.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:215396b75da6fa2f6616f31ea7b6e8dcbb45281d0d9c99c994bf7677751c4003 +size 349750 diff --git a/resources/dual_realsense.gif b/resources/dual_realsense.gif new file mode 100644 index 0000000..7e2b78b --- /dev/null +++ b/resources/dual_realsense.gif @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8914cbaece5eec7a95fc5e16572d791713a178ebe5df2538f5d0d7467e33d1c2 +size 1093533 diff --git a/resources/front_cam_transform.png b/resources/front_cam_transform.png new file mode 100644 index 0000000..453dd90 --- /dev/null +++ b/resources/front_cam_transform.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1f07eab2a6b6d39d1321b05c26f087d2101909c33ff4471f92f9a727f9988148 +size 493460 diff --git a/resources/realsense-viewer_fw-update-recommended.png b/resources/realsense-viewer_fw-update-recommended.png new file mode 100644 index 0000000..42b7b9e --- /dev/null +++ b/resources/realsense-viewer_fw-update-recommended.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9e211f40c0e4a586b48923226baa7ccc7aa9e95d3ee76a7ae0f97a72d675078d +size 151978 diff --git a/resources/realsense.gif b/resources/realsense.gif new file mode 100644 index 0000000..bc39c2b --- /dev/null +++ b/resources/realsense.gif @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c84164c1bf1baa812b67803161b6f9cbb08f5de01ef87a17f03fb356d3d31672 +size 724694 diff --git a/resources/realsense_emitter.png b/resources/realsense_emitter.png new file mode 100644 index 0000000..42aa219 --- /dev/null +++ b/resources/realsense_emitter.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a568cb0a178ef50d6672ed4b9d57d188814b6877a25b228797c1d8249e7b2e62 +size 113300 diff --git a/resources/realsense_viewer_serial_number.png b/resources/realsense_viewer_serial_number.png new file mode 100644 index 0000000..e003c6f --- /dev/null +++ b/resources/realsense_viewer_serial_number.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1eeaf1d3fc2236c948754c22f553a4f30cecd2dbc6c761d115e7cc8e7d32002f +size 695859 diff --git a/resources/vslam_odometry_nav2_diagram.png b/resources/vslam_odometry_nav2_diagram.png new file mode 100644 index 0000000..1d648ab --- /dev/null +++ b/resources/vslam_odometry_nav2_diagram.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6f9c0fbb8d98652d42ff4ab5677af90c358f83c82abdbce8e932b7842b94ca02 +size 45880