diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..40cc34d --- /dev/null +++ b/.gitattributes @@ -0,0 +1,25 @@ +# 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 +*.png filter=lfs diff=lfs merge=lfs -text +*.psd filter=lfs diff=lfs merge=lfs -text + +# Archives +*.gz filter=lfs diff=lfs merge=lfs -text +*.tar filter=lfs diff=lfs merge=lfs -text +*.zip filter=lfs diff=lfs merge=lfs -text + +# Documents +*.pdf filter=lfs diff=lfs merge=lfs -text + +# Shared libraries +*.so filter=lfs diff=lfs merge=lfs -text +*.so.* filter=lfs diff=lfs merge=lfs -text + +# ROS Bags +**/resources/**/*.zstd filter=lfs diff=lfs merge=lfs -text +**/data/**/*.db3 filter=lfs diff=lfs merge=lfs -text +**/data/**/*.yaml filter=lfs diff=lfs merge=lfs -text diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b793570 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +# Ignore all pycache files +**/__pycache__/** diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..a89cd42 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,14 @@ +# Isaac ROS Contribution Rules + +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0.html): + +> **5. Submission of Contributions.** Unless You explicitly state otherwise, any Contribution intentionally submitted for inclusion in the Work by You to the Licensor shall be under the terms and conditions of this License, without any additional terms or conditions. Notwithstanding the above, nothing herein shall supersede or modify the terms of any separate license agreement you may have executed with Licensor regarding such Contributions. + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` +line to commit messages to certify that they have the right to submit +the code they are contributing to the project according to the +[Developer Certificate of Origin (DCO)](https://developercertificate.org/). + +[//]: # (202201002) diff --git a/LICENSE b/LICENSE index 261eeb9..7a4a3ea 100644 --- a/LICENSE +++ b/LICENSE @@ -1,3 +1,4 @@ + Apache License Version 2.0, January 2004 http://www.apache.org/licenses/ @@ -198,4 +199,4 @@ distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and - limitations under the License. + limitations under the License. \ No newline at end of file diff --git a/README.md b/README.md index f260d7f..8d03064 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,365 @@ -# isaac_ros_map_localization -Hardware-accelerated global localization +# Isaac ROS Map Localization + +
Isaac ROS Map Localization Sample Output
+ +## Overview + +The Isaac ROS Map Localization module contains ROS 2 packages for lidar processing to estimate poses relative to a map. The Occupancy Grid Localizer processes a planar range scan to estimate pose in an occupancy grid map; this occurs in less than 1 second for most maps. This initial pose can be used to bootstrap navigation for mobile robots and has been integrated and tested with Nav2. This can remove the need for upwards of 30 seconds to [manually estimate the position and direction]( https://youtu.be/IrJmuow1r7g?t=1029) of a robot with RViz, for example. + +The Occupancy Grid Localizer is designed to work with planar and 3D LIDARs. It uses [Flatscan](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/isaac_ros_pointcloud_interfaces/msg/FlatScan.msg) for input to the GPU-accelerated computation estimating pose. [Flatscan](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/isaac_ros_pointcloud_interfaces/msg/FlatScan.msg) allows for representation of 3D LIDARs, which have variable angular increments between multiple beams. + +
Isaac ROS Map Localization Sample Output
+ +LaserScan to Flatscan provides conversion from [LaserScan](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/LaserScan.msg), which by definition has [equal angle increment](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/LaserScan.msg#L16) between beams, to [Flatscan](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/isaac_ros_pointcloud_interfaces/msg/FlatScan.msg). + +PointCloud to FlatScan provides conversion from [pointcloud]( https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/PointCloud2.msg) output from 3D LIDARs to [Flatscan](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/isaac_ros_pointcloud_interfaces/msg/FlatScan.msg). + +> **Note**: Localization can be performed mulitiple times during navigation. + +> **Note**: The input FlatScan Message header/frame_id is used to get the transform of the lidar with respect to the robot base_link frame. + +> **Note**: The output `localization_result` is the transform of `base_link` with respect to the frame specified in the `loc_result_frame` (map) ROS parameter. + +> **Note**: Localization can be triggered in one of two ways: +> +> 1. Buffer FlatScan messages received on a topic and trigger the localization using an `std_srvs/Empty` service call. +> 2. Trigger localization every time a FlatScan message is sent to a topic. + +Refer to the [Isaac ROS Occupancy Grid Localizer/Usage section](#usage) for more details. + +### Isaac ROS NITROS Acceleration + +This package is powered by [NVIDIA Isaac Transport for ROS (NITROS)](https://developer.nvidia.com/blog/improve-perception-performance-for-ros-2-applications-with-nvidia-isaac-transport-for-ros/), which leverages type adaptation and negotiation to optimize message formats and dramatically accelerate communication between participating nodes. + +## Performance + +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. + +| Sample Graph | Input Size | AGX Orin | Orin NX | Orin Nano 8GB | x86_64 w/ RTX 3060 Ti | +| ---------------------------------------------------------------------------------------------------------------------------------------------------- | ---------- | -------------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------- | +| [Occupancy Grid Localizer Node](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/scripts//isaac_ros_grid_localizer_node.py) | ~50 sq. m | [16.0 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_grid_localizer_node-agx_orin.json)
79 ms | [7.33 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_grid_localizer_node-orin_nx.json)
150 ms | [5.16 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_grid_localizer_node-orin_nano_8gb.json)
220 ms | [46.5 fps](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_benchmark/blob/main/results/isaac_ros_grid_localizer_node-x86_64_rtx_3060Ti.json)
33 ms | + + +## Table of Contents + +- [Isaac ROS Map Localization](#isaac-ros-map-localization) + - [Overview](#overview) + - [Isaac ROS NITROS Acceleration](#isaac-ros-nitros-acceleration) + - [Performance](#performance) + - [Table of Contents](#table-of-contents) + - [Latest Update](#latest-update) + - [Supported Platforms](#supported-platforms) + - [Docker](#docker) + - [Quickstart](#quickstart) + - [Next Steps](#next-steps) + - [Try More Examples](#try-more-examples) + - [Customize your Dev Environment](#customize-your-dev-environment) + - [Package Reference](#package-reference) + - [Isaac ROS Occupancy Grid Localizer](#isaac-ros-occupancy-grid-localizer) + - [Usage](#usage) + - [ROS Parameters](#ros-parameters) + - [ROS Topics Subscribed](#ros-topics-subscribed) + - [ROS Topics Published](#ros-topics-published) + - [ROS Services Advertised](#ros-services-advertised) + - [Isaac ROS PointCloud to FlatScan](#isaac-ros-pointcloud-to-flatscan) + - [Usage](#usage-1) + - [ROS Parameters](#ros-parameters-1) + - [ROS Topics Subscribed](#ros-topics-subscribed-1) + - [ROS Topics Published](#ros-topics-published-1) + - [Isaac ROS LaserScan to FlatScan](#isaac-ros-laserscan-to-flatscan) + - [Usage](#usage-2) + - [ROS Topics Subscribed](#ros-topics-subscribed-2) + - [ROS Topics Published](#ros-topics-published-2) + - [Isaac ROS FlatScan to LaserScan](#isaac-ros-flatscan-to-laserscan) + - [Usage](#usage-3) + - [ROS Parameters](#ros-parameters-2) + - [ROS Topics Subscribed](#ros-topics-subscribed-3) + - [ROS Topics Published](#ros-topics-published-3) + - [Troubleshooting](#troubleshooting) + - [Isaac ROS Troubleshooting](#isaac-ros-troubleshooting) + - [Updates](#updates) + +## Latest Update + +2023-04-05: Added `isaac_ros_occupancy_grid_localizer` + +## Supported Platforms + +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. + +| 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 the [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.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. + +## Quickstart + +1. Set up your development environment by following [these instructions](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`: + + ```bash + cd ~/workspaces/isaac_ros-dev/src + ``` + + ```bash + git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common.git + ``` + + ```bash + git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_nitros.git + ``` + + ```bash + git clone https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_map_localization.git + ``` + +3. Pull down a ROS bag of sample data: + + ```bash + cd ~/workspaces/isaac_ros-dev/src/isaac_ros_map_localization && \ + git lfs pull -X "" -I "**/rosbags/" + ``` + +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: + + ```bash + cd /workspaces/isaac_ros-dev && \ + colcon build --symlink-install && \ + source install/setup.bash + ``` + +6. (Optional) Run tests to verify complete and correct installation: + + ```bash + colcon test --executor sequential + ``` + +7. Run ``rviz2``: + + ```bash + rviz2 -d src/isaac_ros_map_localization/isaac_ros_occupancy_grid_localizer/rviz/quickstart.rviz + ``` + +8. Create another terminal in the Docker container using the `run_dev.sh` script: + + ```bash + cd ~/workspaces/isaac_ros-dev/src/isaac_ros_common && \ + ./scripts/run_dev.sh + ``` + +9. Run the launch file to spin up a demo of this package: + + ```bash + source install/setup.bash && \ + ros2 launch isaac_ros_occupancy_grid_localizer isaac_ros_occupancy_grid_localizer_quickstart.launch.py + ``` + +10. Create another terminal in the Docker container using the `run_dev.sh` script: + + ```bash + cd ~/workspaces/isaac_ros-dev/src/isaac_ros_common && \ + ./scripts/run_dev.sh + ``` + +11. Run the rosbag: + + ```bash + source install/setup.bash && \ + ros2 bag play -l src/isaac_ros_map_localization/isaac_ros_occupancy_grid_localizer/data/rosbags/flatscan + ``` + +12. Create another terminal in the docker container using the `run_dev.sh` script: + + ```bash + cd ~/workspaces/isaac_ros-dev/src/isaac_ros_common && \ + ./scripts/run_dev.sh + ``` + +13. Trigger the localization using a command line service call : + + ```bash + ros2 service call trigger_grid_search_localization std_srvs/srv/Empty {} + ``` + +14. You should see a frame being generated in the map showing the position of the LIDAR. +
Isaac ROS Map Localization Sample Output
+ +## Next Steps + +### Try More Examples + +To continue exploring Isaac ROS MAP localization, check out the following examples: + +- [Isaac ROS Occupancy Grid Localizer Nav2 Isaac Sim Tutorial](./docs/isaac-sim-nav2-tutorial.md) + +### Customize your Dev Environment + +To customize your development environment, refer to [this guide](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/docs/modify-dockerfile.md). + +## Package Reference + +### Isaac ROS Occupancy Grid Localizer + +#### Usage + +```bash +ros2 launch isaac_ros_occupancy_grid_localizer isaac_ros_occupancy_grid_localizer.launch.py +``` + +> **Note**: Use the `flatscan` topic with the `trigger_grid_search_localization` service to trigger localization using a service. +> +> **Or** publish directly to the `flatscan_localization` topic to trigger localization every time a FlatScan message is received on this topic. +> +> **Do not** publish FlatScan messages to both `flatscan` and `flatscan_localization` topics. + +#### ROS Parameters + +> **Note**: The ROS param names are the same as the Nav2 [map_server](https://github.com/ros-planning/navigation2/blob/main/nav2_map_server/src/map_io.cpp#L136) YAML params. This allows to load and pass the same YAML file to both Nav2 and isaac_ros_occupancy_grid_localizer as shown in the [Isaac Sim Launch File](./isaac_ros_occupancy_grid_localizer/launch/isaac_ros_occupancy_grid_localizer_nav2.launch.py) + +| ROS Parameter | Type | Default | Description | +| ------------------------- | --------------------- | ----------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `loc_result_frame` | `std::string` | `map` | frame_id of localization result | +| `resolution` | `double` | `0.05` | The meters per pixel of the png map being loaded. This parameter is loaded from the the map YAML file. | +| `origin` | `std::vector` | `[0.0, 0.0, 0.0]` | The origin of the map loaded. Used to transform the output to compensate for the same transform made to the PNG file loaded by the Nav2 [map_server](https://github.com/ros-planning/navigation2/blob/main/nav2_map_server/src/map_io.cpp#L136). | +| `occupied_thresh` | `double` | `0.65` | Pixels with occupancy probability greater than this threshold are considered completely occupied. This parameter is loaded from the the map yaml file.
Supported values: `[0,1)` | +| `image` | `std::string` | `""` | Name of the PNG file used to load map. This should be in the same directory as the map YAML file specified in `map_yaml_path`. This parameter is loaded from the the map YAML file. | +| `map_yaml_path` | `std::string` | `""` | Absolute path to the map yaml file. From which we load the `resolution` and `occupied_thresh` | +| `max_points` | `int` | `20000` | Maximum number of points in FlatScan Message that can be received used to pre-allocate GPU memory. | +| `robot_radius` | `double` | `0.25` | The radius of the robot. This parameter is used to exclude poses which are too close to an obstacle.memory. | +| `max_beam_error` | `double` | `0.5` | The maximum beam error used when comparing range scans. | +| `num_beams_gpu` | `int` | `512` | The GPU accelerated scan-and-match function can only handle a certain number of beams per range scan. The allowed values are {32, 64, 128, 256, 512}. If the number of beams in the range scan does not match this number a subset of beams will be taken. | +| `batch_size` | `int` | `512` | This is the number of scans to collect into a batch for the GPU kernel. Choose a value which matches your GPU well. | +| `out_of_range_threshold` | `double` | `100.0` | Points range larger than this threshold will be marked as out of range and not used. | +| `invalid_range_threshold` | `double` | `0.0` | Points range smaller than this threshold will be marked as invalid and not used. | +| `min_scan_fov_degrees` | `double` | `270.0` | Minimal required scan fov to run the localizer. | +| `use_closest_beam` | `bool` | `true` | Whether or not pick the closest angle beam in angle bucket, if not pick the average within an angular bucket | +| `min_output_error` | `double` | `0.22` | The minimal output error used to normalize and compute confidence, if output error from best sample smaller or equal to this, the confidence is 1 | +| `max_output_error` | `double` | `0.35` | The max output error from our best sample, if output error larger than this threshold, we conclude localization failed | + +#### ROS Topics Subscribed + +| ROS Topic | Type | Description | +| ----------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `flatscan` | [isaac_ros_pointcloud_interfaces::msg::FlatScan](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/isaac_ros_pointcloud_interfaces/msg/FlatScan.msg) | The input FlatScan messages buffer. The last message on this topic will be used as input for localization when the `trigger_grid_search_localization` service is called. | +| `flatscan_localization` | [isaac_ros_pointcloud_interfaces::msg::FlatScan](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/isaac_ros_pointcloud_interfaces/msg/FlatScan.msg) | The topic to trigger localization directly without a buffer. Localization will be triggered every time a FlatScan message is received on this topic. | + +#### ROS Topics Published + +| ROS Topic | Interface | Description | +| --------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------ | ------------------------------------------------------------------------------------------------------------------------- | +| `localization_result` | [geometry_msgs::msg::PoseWithCovarianceStamped](https://github.com/ros2/common_interfaces/blob/humble/geometry_msgs/msg/PoseWithCovarianceStamped.msg) | Pose of the scan data with respect to the map origin, as specified in the first note in the [overview section](#overview) | + +#### ROS Services Advertised + +| ROS Service | Interface | Description | +| ---------------------------------- | ---------------------------------------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------- | +| `trigger_grid_search_localization` | [std_srvs::srv::Empty](https://github.com/ros2/common_interfaces/blob/humble/std_srvs/srv/Empty.srv) | The service to trigger the global localization using the last scan received on the `flatscan` input topic. | + +### Isaac ROS PointCloud to FlatScan + +#### Usage + +```bash +ros2 launch isaac_ros_pointcloud_utils isaac_ros_pointcloud_to_flatscan.launch.py +``` + +#### ROS Parameters + +| ROS Parameter | Type | Default | Description | +| ------------------ | ------ | -------- | -------------------------------------------------------------------------------- | +| `threshold_x_axis` | bool | `false` | Enable X Axis Threshold | +| `threshold_y_axis` | bool | `false` | Enable Y Axis Threshold | +| `min_x` | double | `-1.0` | Min X axis threshold | +| `max_x` | double | `1.0` | Max X axis threshold | +| `min_y` | double | `-1.0` | Min Y axis threshold | +| `max_y` | double | `1.0` | Max Y axis threshold | +| `min_z` | double | `-0.1` | Min Z axis threshold | +| `max_z` | double | `0.1` | Max Z axis threshold | +| `max_points` | int | `150000` | Maximum number of 3D points in input Point Cloud used to pre allocate GPU memory | + +#### ROS Topics Subscribed + +| ROS Topic | Type | Description | +| ------------ | ---------------------------------------------------------------------------------------------------------------------- | ----------------- | +| `pointcloud` | [sensor_msgs::msg::PointCloud2](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/PointCloud2.msg) | Input Point Cloud | + +#### ROS Topics Published + +| ROS Topic | Type | Description | +| ---------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ---------------- | +| `flatscan` | [isaac_ros_pointcloud_interfaces::msg::FlatScan](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/isaac_ros_pointcloud_interfaces/msg/FlatScan.msg) | Output Flat Scan | + +### Isaac ROS LaserScan to FlatScan + +#### Usage + +```bash +ros2 launch isaac_ros_pointcloud_utils isaac_ros_laserscan_to_flatscan.launch.py +``` + +#### ROS Topics Subscribed + +| ROS Topic | Type | Description | +| --------- | ------------------------------------------------------------------------------------------------------------------ | --------------- | +| `scan` | [sensor_msgs::msg::LaserScan](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/LaserScan.msg) | Input LaserScan | + +#### ROS Topics Published + +| ROS Topic | Type | Description | +| ---------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ---------------- | +| `flatscan` | [isaac_ros_pointcloud_interfaces::msg::FlatScan](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/isaac_ros_pointcloud_interfaces/msg/FlatScan.msg) | Output Flat Scan | + +### Isaac ROS FlatScan to LaserScan + +#### Usage + +```bash +ros2 launch isaac_ros_pointcloud_utils isaac_ros_flatscan_to_laserscan.launch.py +``` + +#### ROS Parameters + +| ROS Parameter | Type | Default | Description | +| -------------------- | ------ | ------------ | ------------------------------------------------------------------------------------------------------------------------------- | +| `angle_min` | double | `0.0` | The starting angle of the generated LaserScan | +| `angle_max` | double | `2 * M_PI` | The ending angle of the generated LaserScan | +| `angle_increment` | double | `M_PI / 180` | The angle increment per LaserScan reading | +| `time_increment` | double | `0.0001` | The time increment per LaserScan reading | +| `max_range_fallback` | double | `200.0` | If the Max Range of the input FlatScan == 0, then this parameter is used to populate 'Max Range' field of the output LaserScan. | + +#### ROS Topics Subscribed + +| ROS Topic | Type | Description | +| ---------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | --------------- | +| `flatscan` | [isaac_ros_pointcloud_interfaces::msg::FlatScan](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/isaac_ros_pointcloud_interfaces/msg/FlatScan.msg) | Input Flat Scan | + +#### ROS Topics Published + +| ROS Topic | Type | Description | +| --------- | ------------------------------------------------------------------------------------------------------------------ | --------------- | +| `scan` | [sensor_msgs::msg::LaserScan](https://github.com/ros2/common_interfaces/blob/humble/sensor_msgs/msg/LaserScan.msg) | Ouput LaserScan | + +## Troubleshooting + +### Isaac ROS Troubleshooting + +For solutions to problems with Isaac ROS, please check [here](https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common/blob/main/docs/troubleshooting.md). + +## Updates + +| Date | Changes | +| ---------- | ------------------------------------------ | +| 2023-04-05 | Added `isaac_ros_occupancy_grid_localizer` | diff --git a/docs/isaac-sim-nav2-tutorial.md b/docs/isaac-sim-nav2-tutorial.md new file mode 100644 index 0000000..575b0c9 --- /dev/null +++ b/docs/isaac-sim-nav2-tutorial.md @@ -0,0 +1,59 @@ +# Isaac ROS Occupancy Grid Localizer Nav2 Isaac Sim Tutorial + +
Isaac ROS Occupancy Grid Localizer Expected Output
+ +## Overview + +This tutorial describes how to integrate the Isaac ROS Occupancy Grid Localizer with a simulated robot running in Isaac Sim with Nav2. + +Last validated with [Isaac Sim 2022.1.0](https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/release_notes.html#id10). + +## Tutorial Walkthrough + +1. Complete the [Quickstart section](../README.md#quickstart) in the main README. +2. Launch the Docker container using the `run_dev.sh` script: + + ```bash + cd ~/workspaces/isaac_ros-dev/src/isaac_ros_common && \ + ./scripts/run_dev.sh + ``` + +3. Inside the container, build and source the workspace: + + ```bash + cd /workspaces/isaac_ros-dev && \ + colcon build --symlink-install && \ + source install/setup.bash + ``` + +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* tab) located at: + + ```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. +6. Press **Play** to start publishing data from the Isaac Sim application. + +7. Run the launch file and you should see a nav2 window pop up. + + ```bash + ros2 launch isaac_ros_occupancy_grid_localizer isaac_ros_occupancy_grid_localizer_nav2.launch.py + ``` + +8. Open a new terminal using the `run_dev.sh` script: + + ```bash + cd ~/workspaces/isaac_ros-dev/src/isaac_ros_common && \ + ./scripts/run_dev.sh + ``` + +9. Trigger the localization service using the command line interface: + + ```bash + ros2 service call /trigger_grid_search_localization std_srvs/srv/Empty {} + ``` + +10. You should see the laser scan in rviz. This happens when the transform between map and carter_lidar frame is available. The robot is now localized. You can give a position setpoint using the `2D Nav Goal` button as shown below. You can relocalize again at any point by running the `ros2 service call` command above. + +
Isaac ROS Occupancy Grid Localizer Expected Output
diff --git a/isaac_ros_occupancy_grid_localizer/CMakeLists.txt b/isaac_ros_occupancy_grid_localizer/CMakeLists.txt new file mode 100644 index 0000000..c0bb21b --- /dev/null +++ b/isaac_ros_occupancy_grid_localizer/CMakeLists.txt @@ -0,0 +1,46 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.23.2) +project(isaac_ros_occupancy_grid_localizer LANGUAGES C CXX) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +# # Dependencies +find_package(Eigen3 3.3 REQUIRED NO_MODULE) + +# OccupancyGridLocalizerNode +ament_auto_add_library(occupancy_grid_localizer SHARED src/occupancy_grid_localizer_node.cpp) +rclcpp_components_register_nodes(occupancy_grid_localizer "nvidia::isaac_ros::occupancy_grid_localizer::OccupancyGridLocalizerNode") +set(node_plugins "${node_plugins}nvidia::isaac_ros::occupancy_grid_localizer::OccupancyGridLocalizerNode;\ + $\n") +target_link_libraries(occupancy_grid_localizer Eigen3::Eigen) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(launch_testing_ament_cmake REQUIRED) + add_launch_test(test/isaac_ros_occupancy_grid_localizer_pol_test.py) +endif() + +ament_auto_package(INSTALL_TO_SHARE config launch maps params rviz) diff --git a/isaac_ros_occupancy_grid_localizer/config/namespace_injector_rule.yaml b/isaac_ros_occupancy_grid_localizer/config/namespace_injector_rule.yaml new file mode 100644 index 0000000..12b6b82 --- /dev/null +++ b/isaac_ros_occupancy_grid_localizer/config/namespace_injector_rule.yaml @@ -0,0 +1,30 @@ +%YAML 1.2 +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 +--- +name: OccupancyGridLocalizer Node Namespace Injector Rule +operation: namespace_injector +body: + components: + - type: nvidia::isaac::AtlasFrontend + path_parameter_keys: [occupancy_grid_map] + - type: nvidia::isaac::localization::Occupancy2DMapProvider + path_parameter_keys: [atlas] + - type: nvidia::isaac::localization::GridSearchLocalizer + path_parameter_keys: [atlas, occupancy_2d_map_provider] + - type: nvidia::isaac::TensorCopier + path_parameter_keys: [cuda_stream] diff --git a/isaac_ros_occupancy_grid_localizer/config/occupancy_grid_localizer_node.yaml b/isaac_ros_occupancy_grid_localizer/config/occupancy_grid_localizer_node.yaml new file mode 100644 index 0000000..ac12bd1 --- /dev/null +++ b/isaac_ros_occupancy_grid_localizer/config/occupancy_grid_localizer_node.yaml @@ -0,0 +1,279 @@ +%YAML 1.2 +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 +--- +############### +# Cuda Stream # +############### +name: cuda_stream +components: +- name: cuda_stream + type: nvidia::isaac::CudaStream + parameters: + allocate_stream: true +--- +############################### +# Binary Occupancy Map Loader # +############################### +name: binary_occupancy_map_loader +components: +- name: binary_occupancy_map + type: nvidia::gxf::Tensor +- name: host_allocator + type: nvidia::gxf::UnboundedAllocator +- name: device_allocator + type: nvidia::gxf::UnboundedAllocator +- name: image_loader + type: nvidia::isaac::ImageLoader + parameters: + allocator: host_allocator + tensor: binary_occupancy_map + # Will be overidden with png file in ROS Param yaml file specified in ROS parameter 'map_yaml_path' + filename: "" +- name: occupancy_grid_map + type: nvidia::isaac::OccupancyGridMap + parameters: + cell_size: 0.05 # Will be overidden with ROS parameter 'resolution' + threshold: 200 # Will be overidden with ROS parameter 'occupancy_grid_map_threshold' + binary_occupancy_map_tensor: binary_occupancy_map + map_frame: global_pose_tree/map_frame + device_allocator: device_allocator +- type: nvidia::gxf::CountSchedulingTerm + parameters: + count: 1 +--- +################## +# Atlas Frontend # +################## +name: atlas +components: + - name: frontend + type: nvidia::isaac::AtlasFrontend + parameters: + pose_tree: global_pose_tree/pose_tree + occupancy_grid_map: binary_occupancy_map_loader/occupancy_grid_map +--- +################################# +# Binary Occupancy Map Provider # +################################# +name: occupancy_2d_map_provider +components: + - name: occupancy_2d_map_provider + type: nvidia::isaac::localization::Occupancy2DMapProvider + parameters: + atlas: atlas/frontend + map_frame: global_pose_tree/map_frame + world_frame: global_pose_tree/world_frame +--- +########################################### +# Localization to Pose3D Format Converter # +########################################### +name: localization_to_pose3d +components: + - name: rx_localization_result + type: nvidia::gxf::DoubleBufferReceiver + parameters: + policy: 0 + - name: allocator + type: nvidia::gxf::BlockMemoryPool + parameters: + storage_type: 0 + block_size: 1000 + num_blocks: 4 + - name: message_scheduling_term + type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: rx_localization_result + min_size: 1 + - name: tx_pose3d_cov + type: nvidia::gxf::DoubleBufferTransmitter + parameters: + policy: 0 + - name: localization_to_pose3d + type: nvidia::isaac::localization::LocalizationToPose3d + parameters: + rx_localization_result: rx_localization_result + tx_pose3d_cov: tx_pose3d_cov + allocator: allocator +--- +####################### +# Global Localization # +####################### +name: global_localization +components: +- name: rx_flatscan + type: nvidia::gxf::DoubleBufferReceiver + parameters: + policy: 0 +- name: message_scheduling_term + type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: rx_flatscan + min_size: 1 +- name: rx_reset_signal + type: nvidia::gxf::DoubleBufferReceiver + parameters: + policy: 0 +- name: message_scheduling_term + type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: rx_reset_signal + min_size: 1 +- name: tx_localization_result + type: nvidia::gxf::DoubleBufferTransmitter + parameters: + policy: 0 +- name: grid_search_localizer + type: nvidia::isaac::localization::GridSearchLocalizer + parameters: + dummy_rx: rx_flatscan + flatscan_receivers: [rx_flatscan] + rx_reset_signal: rx_reset_signal + atlas: atlas/frontend + robot_frame: global_pose_tree/robot_frame + world_frame: global_pose_tree/world_frame + tx_localization_result: tx_localization_result + occupancy_2d_map_provider: occupancy_2d_map_provider/occupancy_2d_map_provider + robot_radius: 0.25 + max_beam_error: 0.5 + num_beams_gpu: 512 + batch_size: 512 + out_of_range_threshold: 100.0 + invalid_range_threshold: 0.0 + min_scan_fov_degrees: 270.0 + use_closest_beam: true + min_output_error: 0.22 + max_output_error: 0.35 +--- +name: broadcaster +components: +- type: nvidia::gxf::Broadcast + parameters: + source: rx_flatscan +- name: rx_flatscan + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 1 + policy: 0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: rx_flatscan + min_size: 1 +- name: tx_reset_signal + type: nvidia::gxf::DoubleBufferTransmitter + parameters: + capacity: 1 +- type: nvidia::gxf::DownstreamReceptiveSchedulingTerm + parameters: + transmitter: tx_reset_signal + min_size: 1 +- name: tx_flatscan + type: nvidia::gxf::DoubleBufferTransmitter + parameters: + capacity: 1 +- type: nvidia::gxf::DownstreamReceptiveSchedulingTerm + parameters: + transmitter: tx_flatscan + min_size: 1 +--- +name: flatscan_beams_tensor_copier +components: +- name: rx_flatscan + type: nvidia::gxf::DoubleBufferReceiver +- name: tx_flatscan + type: nvidia::gxf::DoubleBufferTransmitter +- type: nvidia::gxf::DownstreamReceptiveSchedulingTerm + parameters: + transmitter: tx_flatscan + min_size: 1 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: rx_flatscan + min_size: 1 +- name: allocator + type: nvidia::gxf::BlockMemoryPool + parameters: + storage_type: 0 + block_size: 19200000 + num_blocks: 120 +- name: tensor_copier + type: nvidia::isaac::TensorCopier + parameters: + receiver: rx_flatscan + transmitter: tx_flatscan + tensor_name: beams + rx_memory_type: 1 + tx_memory_type: 0 + allocator: allocator + cuda_stream: cuda_stream/cuda_stream +--- +name: vault +components: +- name: signal + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 1 + policy: 0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: signal + min_size: 1 +- name: vault + type: nvidia::gxf::Vault + parameters: + source: signal + max_waiting_count: 1 + drop_waiting: false +--- +components: +- name: edge0 + type: nvidia::gxf::Connection + parameters: + source: flatscan_beams_tensor_copier/tx_flatscan + target: broadcaster/rx_flatscan +- name: edge1 + type: nvidia::gxf::Connection + parameters: + source: broadcaster/tx_flatscan + target: global_localization/rx_flatscan +- name: edge2 + type: nvidia::gxf::Connection + parameters: + source: broadcaster/tx_reset_signal + target: global_localization/rx_reset_signal +- name: edge3 + type: nvidia::gxf::Connection + parameters: + source: global_localization/tx_localization_result + target: localization_to_pose3d/rx_localization_result +- name: edge4 + type: nvidia::gxf::Connection + parameters: + source: localization_to_pose3d/tx_pose3d_cov + target: vault/signal +--- +components: +- name: clock + type: nvidia::gxf::RealtimeClock +- type: nvidia::gxf::MultiThreadScheduler + parameters: + clock: clock + worker_thread_number: 8 + stop_on_deadlock: false +- type: nvidia::gxf::JobStatistics + parameters: + clock: clock diff --git a/isaac_ros_occupancy_grid_localizer/data/rosbags/flatscan/flatscan_0.db3 b/isaac_ros_occupancy_grid_localizer/data/rosbags/flatscan/flatscan_0.db3 new file mode 100644 index 0000000..09b599c --- /dev/null +++ b/isaac_ros_occupancy_grid_localizer/data/rosbags/flatscan/flatscan_0.db3 @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c53f2dc2beab6ab255405229286fe2da2cce25ec88b3d2abdbe2953e6c4a2ee4 +size 12288000 diff --git a/isaac_ros_occupancy_grid_localizer/data/rosbags/flatscan/metadata.yaml b/isaac_ros_occupancy_grid_localizer/data/rosbags/flatscan/metadata.yaml new file mode 100644 index 0000000..db8b1a5 --- /dev/null +++ b/isaac_ros_occupancy_grid_localizer/data/rosbags/flatscan/metadata.yaml @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:76d0998287220a4eccc38e9cee786dec46dbf05eb516cde92b9164dfcfde1a13 +size 996 diff --git a/isaac_ros_occupancy_grid_localizer/include/isaac_ros_occupancy_grid_localizer/occupancy_grid_localizer_node.hpp b/isaac_ros_occupancy_grid_localizer/include/isaac_ros_occupancy_grid_localizer/occupancy_grid_localizer_node.hpp new file mode 100644 index 0000000..a724619 --- /dev/null +++ b/isaac_ros_occupancy_grid_localizer/include/isaac_ros_occupancy_grid_localizer/occupancy_grid_localizer_node.hpp @@ -0,0 +1,132 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#ifndef ISAAC_ROS_OCCUPANCY_GRID_LOCALIZER__OCCUPANCY_GRID_LOCALIZER_NODE_HPP_ +#define ISAAC_ROS_OCCUPANCY_GRID_LOCALIZER__OCCUPANCY_GRID_LOCALIZER_NODE_HPP_ + +#include +#include +#include +#include +#include + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "isaac_ros_nitros/nitros_node.hpp" +#include "isaac_ros_nitros/types/nitros_type_base.hpp" +#include "isaac_ros_pointcloud_interfaces/msg/flat_scan.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_srvs/srv/empty.hpp" +#include "tf2/exceptions.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" + +namespace nvidia +{ +namespace isaac_ros +{ +namespace occupancy_grid_localizer +{ + +class OccupancyGridLocalizerNode : public nitros::NitrosNode +{ +public: + explicit OccupancyGridLocalizerNode(const rclcpp::NodeOptions &); + + ~OccupancyGridLocalizerNode(); + + OccupancyGridLocalizerNode(const OccupancyGridLocalizerNode &) = delete; + + OccupancyGridLocalizerNode & operator=(const OccupancyGridLocalizerNode &) = delete; + + // The callback to be implemented by users for any required initialization + void postLoadGraphCallback() override; + + void GridSearchLocalizationCallback( + const std::shared_ptr, + std::shared_ptr); + + void FlatScanCallback( + const std::shared_ptr flat_scan); + + void LocalizationResultCallback(const gxf_context_t context, nitros::NitrosTypeBase & msg); + + // Callback linked to FlatScan subscriber + // used to populate atlas with transform on atlas wrt to robot + void FlatScanNitrosSubCallback(const gxf_context_t, nitros::NitrosTypeBase &); + + // Callback linked to Pose3dwitCov publisher + // used to convert the result from Isaac map standard to ROS map_laoder standard + void LocResultNitrosSubCallback(const gxf_context_t, nitros::NitrosTypeBase &); + +private: + std::optional> last_flat_scan_; + + rclcpp::Publisher::SharedPtr flat_scan_publisher_; + + rclcpp::Subscription::SharedPtr + flat_scan_subscriber_; + + rclcpp::Service::SharedPtr grid_search_localize_service_server_; + + // read the image height without loading the image + void GePngImageHeight(const std::string & file_path, unsigned int & height); + + const double cell_size_; + const int occupancy_grid_map_threshold_; + const std::string map_yaml_path_; + const std::string map_png_path_; + const std::vector map_origin_; + const int max_points_; + const bool use_gxf_map_convention_; + // The radius of the robot. This parameter is used to exclude poses which are too close to an + // obstacle. + const double robot_radius_; + // The maximum beam error used when comparing range scans. + const double max_beam_error_; + // The GPU accelerated scan-and-match function can only handle a certain number of beams per + // range scan. The allowed values are {32, 64, 128, 256, 512}. If the number of beams in the + // range scan does not match this number a subset of beams will be taken. + const int num_beams_gpu_; + // This is the number of scans to collect into a batch for the GPU kernel. Choose a value which + // matches your GPU well. + const int batch_size_; + // Points range larger than this threshold will be marked as out of range and not used. + const double out_of_range_threshold_; + // Points range smaller than this threshold will be marked as invalid and not used. + const double invalid_range_threshold_; + // Minimal required scan fov to run the localizer. + const double min_scan_fov_degrees_; + // Whether or not pick the closest angle beam in angle bucket, if not pick the average within an + // angular bucket + const bool use_closest_beam_; + // The minimal output error used to normalize and compute confidence, if output error from best + // sample smaller or equal to this, the confidence is 1 + const double min_output_error_; + // The max output error from our best sample, if output error larger than this threshold, we + // conclude localization failed + const double max_output_error_; + + unsigned int map_png_height_; + std::shared_ptr tf_listener_; + std::unique_ptr tf_buffer_; +}; + +} // namespace occupancy_grid_localizer +} // namespace isaac_ros +} // namespace nvidia + +#endif // ISAAC_ROS_OCCUPANCY_GRID_LOCALIZER__OCCUPANCY_GRID_LOCALIZER_NODE_HPP_ diff --git a/isaac_ros_occupancy_grid_localizer/launch/isaac_ros_occupancy_grid_localizer.launch.py b/isaac_ros_occupancy_grid_localizer/launch/isaac_ros_occupancy_grid_localizer.launch.py new file mode 100644 index 0000000..f14fd1a --- /dev/null +++ b/isaac_ros_occupancy_grid_localizer/launch/isaac_ros_occupancy_grid_localizer.launch.py @@ -0,0 +1,63 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + + map_yaml_path = \ + LaunchConfiguration('map_yaml_path', + default=os.path.join( + get_package_share_directory( + 'isaac_ros_occupancy_grid_localizer'), + 'maps', 'map.yaml')) + + map_yaml_path_arg = DeclareLaunchArgument( + 'map_yaml_path', + default_value=map_yaml_path + ) + + occupancy_grid_localizer_node = ComposableNode( + package='isaac_ros_occupancy_grid_localizer', + plugin='nvidia::isaac_ros::occupancy_grid_localizer::OccupancyGridLocalizerNode', + name='occupancy_grid_localizer', + parameters=[map_yaml_path, { + 'loc_result_frame': 'map', + 'map_yaml_path': map_yaml_path, + }]) + + occupancy_grid_localizer_container = ComposableNodeContainer( + package='rclcpp_components', + name='occupancy_grid_localizer_container', + namespace='', + executable='component_container_mt', + composable_node_descriptions=[ + occupancy_grid_localizer_node, + ], + output='screen' + ) + + return LaunchDescription([map_yaml_path_arg, + occupancy_grid_localizer_container]) diff --git a/isaac_ros_occupancy_grid_localizer/launch/isaac_ros_occupancy_grid_localizer_nav2.launch.py b/isaac_ros_occupancy_grid_localizer/launch/isaac_ros_occupancy_grid_localizer_nav2.launch.py new file mode 100644 index 0000000..798bb1f --- /dev/null +++ b/isaac_ros_occupancy_grid_localizer/launch/isaac_ros_occupancy_grid_localizer_nav2.launch.py @@ -0,0 +1,135 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2021-2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer, Node +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + + map_file_arg = DeclareLaunchArgument( + 'map_file', default_value=os.path.join( + get_package_share_directory( + 'isaac_ros_occupancy_grid_localizer'), 'maps', 'isaac_sim.yaml'), + description='Full path to map file to load') + params_file_arg = DeclareLaunchArgument( + 'params_file', default_value=os.path.join( + get_package_share_directory( + 'isaac_ros_occupancy_grid_localizer'), 'params', 'carter_nav2.yaml'), + description='Full path to param file to load') + use_sim_time_arg = DeclareLaunchArgument( + 'use_sim_time', default_value='True', + description='Use simulation (Omniverse Isaac Sim) clock if true') + run_rviz_arg = DeclareLaunchArgument( + 'run_rviz', default_value='True', + description='Whether to start RVIZ') + run_nav2_arg = DeclareLaunchArgument( + 'run_nav2', default_value='True', + description='Whether to run nav2') + + nav2_bringup_launch_dir = os.path.join( + get_package_share_directory('nav2_bringup'), 'launch') + rviz_config_dir = os.path.join(get_package_share_directory( + 'isaac_ros_occupancy_grid_localizer'), 'rviz', + 'isaac_sim_nav2.rviz') + + rviz_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_launch_dir, 'rviz_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'autostart': 'True', + 'rviz_config': rviz_config_dir + }.items(), + condition=IfCondition(LaunchConfiguration('run_rviz'))) + + nav2_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_launch_dir, 'bringup_launch.py')), + launch_arguments={ + 'map': LaunchConfiguration('map_file'), + 'use_sim_time': LaunchConfiguration('use_sim_time'), + 'params_file': LaunchConfiguration('params_file') + }.items(), + condition=IfCondition(LaunchConfiguration('run_nav2')) + ) + + occupancy_grid_localizer_node = ComposableNode( + package='isaac_ros_occupancy_grid_localizer', + plugin='nvidia::isaac_ros::occupancy_grid_localizer::OccupancyGridLocalizerNode', + name='occupancy_grid_localizer', + parameters=[LaunchConfiguration('map_file'), { + 'loc_result_frame': 'map', + 'map_yaml_path': LaunchConfiguration('map_file'), + }], + remappings=[('localization_result', '/initialpose')]) + + laserscan_to_flatscan_node = ComposableNode( + package='isaac_ros_pointcloud_utils', + plugin='nvidia::isaac_ros::pointcloud_utils::LaserScantoFlatScanNode', + name='laserscan_to_flatscan') + + occupancy_grid_localizer_container = ComposableNodeContainer( + package='rclcpp_components', + name='occupancy_grid_localizer_container', + namespace='', + executable='component_container_mt', + composable_node_descriptions=[ + occupancy_grid_localizer_node, + laserscan_to_flatscan_node + ], + output='screen' + ) + + baselink_basefootprint_publisher = Node( + package='tf2_ros', executable='static_transform_publisher', + parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}], + output='screen', + arguments=['0.0', '0.0', '-0.3', '0.0', '0.0', '0.0', 'base_link', + 'base_footprint'], + condition=IfCondition(LaunchConfiguration('run_nav2'))) + + # Seting transform between lidar_frame and base_link + # since Isaac Sim does not set this transform + baselink_lidar_publisher = Node( + package='tf2_ros', executable='static_transform_publisher', + parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}], + output='screen', + arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'base_link', + 'lidar_frame']) + + return LaunchDescription([ + map_file_arg, + params_file_arg, + use_sim_time_arg, + run_rviz_arg, + run_nav2_arg, + rviz_launch, + nav2_launch, + baselink_basefootprint_publisher, + baselink_lidar_publisher, + occupancy_grid_localizer_container + ]) diff --git a/isaac_ros_occupancy_grid_localizer/launch/isaac_ros_occupancy_grid_localizer_quickstart.launch.py b/isaac_ros_occupancy_grid_localizer/launch/isaac_ros_occupancy_grid_localizer_quickstart.launch.py new file mode 100644 index 0000000..24541cd --- /dev/null +++ b/isaac_ros_occupancy_grid_localizer/launch/isaac_ros_occupancy_grid_localizer_quickstart.launch.py @@ -0,0 +1,90 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import GroupAction +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + + lifecycle_nodes = ['map_server'] + map_yaml_path_dir = os.path.join( + get_package_share_directory('isaac_ros_occupancy_grid_localizer'), 'maps', 'map.yaml') + + occupancy_grid_localizer_node = ComposableNode( + package='isaac_ros_occupancy_grid_localizer', + plugin='nvidia::isaac_ros::occupancy_grid_localizer::OccupancyGridLocalizerNode', + name='occupancy_grid_localizer', + parameters=[map_yaml_path_dir, { + 'loc_result_frame': 'map', + 'map_yaml_path': map_yaml_path_dir, + }]) + + occupancy_grid_localizer_container = ComposableNodeContainer( + package='rclcpp_components', + name='occupancy_grid_localizer_container', + namespace='', + executable='component_container_mt', + composable_node_descriptions=[ + occupancy_grid_localizer_node, + ], + output='screen' + ) + + # Seting transform between lidar_frame and base_link + # since Isaac Sim does not set this transform + baselink_lidar_publisher = Node( + package='tf2_ros', executable='static_transform_publisher', + output='screen', + arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'base_link', + 'lidar_frame']) + + load_nodes = GroupAction( + actions=[ + Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + respawn_delay=2.0, + parameters=[{'yaml_filename': map_yaml_path_dir}]), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_localization', + output='screen', + parameters=[{'use_sim_time': False}, + {'autostart': True}, + {'node_names': lifecycle_nodes}]) + ] + ) + + # Create the launch description and populate + ld = LaunchDescription([occupancy_grid_localizer_container, + baselink_lidar_publisher]) + + # Add the actions to launch all of the localiztion nodes + ld.add_action(load_nodes) + + return ld diff --git a/isaac_ros_occupancy_grid_localizer/maps/isaac_sim.png b/isaac_ros_occupancy_grid_localizer/maps/isaac_sim.png new file mode 100644 index 0000000..2a850b3 --- /dev/null +++ b/isaac_ros_occupancy_grid_localizer/maps/isaac_sim.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7c152bfa6909e5eb8f9cce7eab785e8a3c5bd683bb74e9d954ccbf6b84971099 +size 3215 diff --git a/isaac_ros_occupancy_grid_localizer/maps/isaac_sim.yaml b/isaac_ros_occupancy_grid_localizer/maps/isaac_sim.yaml new file mode 100644 index 0000000..0eed87b --- /dev/null +++ b/isaac_ros_occupancy_grid_localizer/maps/isaac_sim.yaml @@ -0,0 +1,7 @@ +image: isaac_sim.png +mode: trinary +resolution: 0.05 +origin: [0.0, 0.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/isaac_ros_occupancy_grid_localizer/maps/map.png b/isaac_ros_occupancy_grid_localizer/maps/map.png new file mode 100644 index 0000000..4c7e666 --- /dev/null +++ b/isaac_ros_occupancy_grid_localizer/maps/map.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:77ee7b74d17ed41242587ca48a17429f14159cd6e29695d2d228d047260a14b4 +size 14487 diff --git a/isaac_ros_occupancy_grid_localizer/maps/map.yaml b/isaac_ros_occupancy_grid_localizer/maps/map.yaml new file mode 100644 index 0000000..ee434a9 --- /dev/null +++ b/isaac_ros_occupancy_grid_localizer/maps/map.yaml @@ -0,0 +1,7 @@ +image: map.png +mode: trinary +resolution: 0.05 +origin: [0.0, 0.0, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/isaac_ros_occupancy_grid_localizer/package.xml b/isaac_ros_occupancy_grid_localizer/package.xml new file mode 100644 index 0000000..3ed1408 --- /dev/null +++ b/isaac_ros_occupancy_grid_localizer/package.xml @@ -0,0 +1,52 @@ + + + + + + + isaac_ros_occupancy_grid_localizer + 0.30.0 + Occupancy Grid Global Localizer. + + Ashwin Varghese Kuruttukulam + Apache-2.0 + https://developer.nvidia.com/isaac-ros/ + Ashwin Varghese Kuruttukulam + + ament_cmake_auto + + rclcpp + rclcpp_components + tf2_ros + isaac_ros_pointcloud_interfaces + std_srvs + isaac_ros_nitros + isaac_ros_nitros_pose_cov_stamped_type + isaac_ros_nitros_flat_scan_type + + isaac_ros_common + + ament_lint_auto + ament_lint_common + isaac_ros_test + + + ament_cmake + + diff --git a/isaac_ros_occupancy_grid_localizer/params/carter_nav2.yaml b/isaac_ros_occupancy_grid_localizer/params/carter_nav2.yaml new file mode 100644 index 0000000..e4c66b9 --- /dev/null +++ b/isaac_ros_occupancy_grid_localizer/params/carter_nav2.yaml @@ -0,0 +1,305 @@ +%YAML 1.2 +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2022 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 +--- +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 20 + default_server_timeout: 40 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + +bt_navigator_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 10.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugin: ["general_goal_checker"] + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + general_goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 2.0 + max_vel_y: 0.0 + max_vel_theta: 2.0 + min_speed_xy: 0.0 + max_speed_xy: 2.0 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 1.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -1.2 + vx_samples: 30 + vy_samples: 10 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 1.0 + RotateToGoal.lookahead_time: -1.0 + +controller_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 10.0 + publish_frequency: 10.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + rolling_window: True + width: 10 + height: 10 + resolution: 0.05 + robot_radius: 0.7 + plugins: ["obstacle_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 10.0 + inflation_radius: 1.0 + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + always_send_full_costmap: True + local_costmap_client: + ros__parameters: + use_sim_time: True + local_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 10.0 + publish_frequency: 10.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + rolling_window: True + width: 200 + height: 200 + robot_radius: 0.7 + resolution: 0.05 + # origin_x: -100.0 + # origin_y: -100.0 + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 10.0 + raytrace_min_range: 0.0 + obstacle_max_range: 10.0 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 10.0 + inflation_radius: 1.0 + always_send_full_costmap: True + global_costmap_client: + ros__parameters: + use_sim_time: True + global_costmap_rclcpp_node: + ros__parameters: + use_sim_time: True + +map_server: + ros__parameters: + use_sim_time: False + yaml_filename: "turtlebot3_world.yaml" + +map_saver: + ros__parameters: + use_sim_time: False + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 10.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +planner_server_rclcpp_node: + ros__parameters: + use_sim_time: True + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 5.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + global_frame: map + robot_base_frame: base_link + transform_tolerance: 0.2 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 2.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + diff --git a/isaac_ros_occupancy_grid_localizer/rviz/isaac_sim_nav2.rviz b/isaac_ros_occupancy_grid_localizer/rviz/isaac_sim_nav2.rviz new file mode 100644 index 0000000..f3f136d --- /dev/null +++ b/isaac_ros_occupancy_grid_localizer/rviz/isaac_sim_nav2.rviz @@ -0,0 +1,565 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /TF1/Tree1 + Splitter Ratio: 0.5833333134651184 + Tree Height: 276 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: nav2_rviz_plugins/Navigation 2 + Name: Navigation 2 +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 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: "" + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: false + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_footprint: + Value: true + base_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + 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: Bumper Hit + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.07999999821186066 + Style: Spheres + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /mobile_base/sensors/bumper_pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Map + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Class: nav2_rviz_plugins/ParticleCloud + Color: 0; 180; 0 + Enabled: true + Max Arrow Length: 0.30000001192092896 + Min Arrow Length: 0.019999999552965164 + Name: Amcl Particle Swarm + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particle_cloud + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Global Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Downsampled Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.019999999552965164 + Head Length: 0.019999999552965164 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: Arrows + Radius: 0.029999999329447746 + Shaft Diameter: 0.004999999888241291 + Shaft Length: 0.019999999552965164 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + 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: 125; 125; 125 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: VoxelGrid + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + Enabled: true + Name: Global Planner + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Local Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 12; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Local Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_plan + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Trajectories + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /marker + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/published_footprint + 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: VoxelGrid + 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: /local_costmap/voxel_marked_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Controller + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RealsenseCamera + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /intel_realsense_r200_depth/image_raw + 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: RealsenseDepthImage + 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: /intel_realsense_r200_depth/points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: false + Name: Realsense + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /waypoints + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/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/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: nav2_rviz_plugins/GoalTool + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: 1.5649993419647217 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 30.007431030273438 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 12.698732376098633 + Y: 14.90666675567627 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1044 + Hide Left Dock: false + Hide Right Dock: true + Navigation 2: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001dc00000382fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b000000ab00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000069000001780000017800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000001ed000001fe000001fe00fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000004500ffffff000000010000010f0000034afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000034a0000012300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004210000038200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RealsenseCamera: + collapsed: false + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1545 + X: 1110 + Y: 113 diff --git a/isaac_ros_occupancy_grid_localizer/rviz/quickstart.rviz b/isaac_ros_occupancy_grid_localizer/rviz/quickstart.rviz new file mode 100644 index 0000000..a73b27a --- /dev/null +++ b/isaac_ros_occupancy_grid_localizer/rviz/quickstart.rviz @@ -0,0 +1,189 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 275 + - 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 + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 255; 25; 0 + 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 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization_result + 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: 61.61069869995117 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 21.15060806274414 + Y: 10.11668872833252 + Z: -14.3422269821167 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.700405597686768 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 2272 + Hide Left Dock: true + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001dc000007f1fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000ab00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000069000007f10000017800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000014f000007f1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000069000007f10000012300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000e6c00000051fc0100000002fb0000000800540069006d0065010000000000000e6c0000045300fffffffb0000000800540069006d0065010000000000000450000000000000000000000e6c000007f100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 3692 + X: 148 + Y: 54 diff --git a/isaac_ros_occupancy_grid_localizer/src/occupancy_grid_localizer_node.cpp b/isaac_ros_occupancy_grid_localizer/src/occupancy_grid_localizer_node.cpp new file mode 100644 index 0000000..9b99782 --- /dev/null +++ b/isaac_ros_occupancy_grid_localizer/src/occupancy_grid_localizer_node.cpp @@ -0,0 +1,443 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include + +#include +namespace fs = std::filesystem; + +#include "isaac_ros_occupancy_grid_localizer/occupancy_grid_localizer_node.hpp" +#include "isaac_ros_nitros_pose_cov_stamped_type/nitros_pose_cov_stamped.hpp" +#include "isaac_ros_nitros_flat_scan_type/nitros_flat_scan.hpp" +#include "isaac_ros_nitros/types/type_adapter_nitros_context.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-parameter" +#pragma GCC diagnostic ignored "-Wmissing-field-initializers" +#pragma GCC diagnostic ignored "-Wpedantic" +#include "engine/core/math/pose3.hpp" +#include "engine/core/math/types.hpp" +#include "extensions/atlas/pose_tree_frame.hpp" +#include "gems/common/pose_frame_uid.hpp" +#include "gems/pose_tree/pose_tree.hpp" +#pragma GCC diagnostic pop + +namespace +{ +constexpr char kPoseTreeEntityName[] = "global_pose_tree"; +constexpr char kPoseTreeComponentName[] = "pose_tree"; +constexpr char kPoseTreeComponentTypeName[] = "nvidia::isaac::PoseTree"; +constexpr char kRobotFrameComponentName[] = "robot_frame"; +constexpr char kRobotFrameComponentTypeName[] = "nvidia::isaac::PoseTreeFrame"; +// Name for the PoseFrameUid in FlatScan message +constexpr char kNamePoseFrameUid[] = "pose_frame_uid"; +constexpr char kBaseLinkFrameName[] = "base_link"; +// Name for the Pose3d in Pose3dCov message +constexpr char kPoseName[] = "pose"; +// Image pixel max value +constexpr int kPixelMax = 255; +// number of components in FlatScan gxf msg +constexpr int kFlatScanGxfMsgSize = 5; +constexpr int kMsgBufferSize = 10; +} // namespace + +namespace nvidia +{ +namespace isaac_ros +{ +namespace occupancy_grid_localizer +{ + +using nvidia::gxf::optimizer::GraphIOGroupSupportedDataTypesInfoList; + +constexpr char INPUT_COMPONENT_KEY_FLATSCAN[] = "flatscan_beams_tensor_copier/rx_flatscan"; +constexpr char INPUT_DEFAULT_TENSOR_FORMAT_FLATSCAN[] = "nitros_flat_scan"; +constexpr char INPUT_TOPIC_NAME_FLATSCAN[] = "flatscan_localization"; + +constexpr char OUTPUT_COMPONENT_KEY_LOC_RESULT[] = "vault/vault"; +constexpr char OUTPUT_DEFAULT_TENSOR_FORMAT_LOC_RESULT[] = "nitros_pose_cov_stamped"; +constexpr char OUTPUT_TOPIC_NAME_LOC_RESULT[] = "localization_result"; +constexpr char OUTPUT_FRAME_ID_MAP_KEY[] = "map"; + +constexpr char APP_YAML_FILENAME[] = "config/occupancy_grid_localizer_node.yaml"; +constexpr char PACKAGE_NAME[] = "isaac_ros_occupancy_grid_localizer"; + +const std::vector> EXTENSIONS = { + {"isaac_ros_gxf", "gxf/lib/serialization/libgxf_serialization.so"}, + {"isaac_ros_gxf", "gxf/lib/libgxf_cuda.so"}, + {"isaac_ros_gxf", "gxf/lib/libgxf_utils.so"}, + {"isaac_ros_gxf", "gxf/lib/libgxf_localization.so"}, + {"isaac_ros_gxf", "gxf/lib/libgxf_flatscan_localization.so"} +}; +const std::vector PRESET_EXTENSION_SPEC_NAMES = { + "isaac_ros_occupancy_grid_localizer", +}; +const std::vector EXTENSION_SPEC_FILENAMES = {}; +const std::vector GENERATOR_RULE_FILENAMES = { + "config/namespace_injector_rule.yaml" +}; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wpedantic" + +const nitros::NitrosPublisherSubscriberConfigMap CONFIG_MAP = { + {INPUT_COMPONENT_KEY_FLATSCAN, + { + .type = nitros::NitrosPublisherSubscriberType::NEGOTIATED, + .qos = rclcpp::QoS(10), + .compatible_data_format = INPUT_DEFAULT_TENSOR_FORMAT_FLATSCAN, + .topic_name = INPUT_TOPIC_NAME_FLATSCAN, + } + }, + {OUTPUT_COMPONENT_KEY_LOC_RESULT, + { + .type = nitros::NitrosPublisherSubscriberType::NEGOTIATED, + .qos = rclcpp::QoS(10), + .compatible_data_format = OUTPUT_DEFAULT_TENSOR_FORMAT_LOC_RESULT, + .topic_name = OUTPUT_TOPIC_NAME_LOC_RESULT, + .frame_id_source_key = OUTPUT_FRAME_ID_MAP_KEY, + } + } +}; +#pragma GCC diagnostic pop + +OccupancyGridLocalizerNode::OccupancyGridLocalizerNode(const rclcpp::NodeOptions & options) +: nitros::NitrosNode(options, + APP_YAML_FILENAME, + CONFIG_MAP, + PRESET_EXTENSION_SPEC_NAMES, + EXTENSION_SPEC_FILENAMES, + GENERATOR_RULE_FILENAMES, + EXTENSIONS, + PACKAGE_NAME), + cell_size_(declare_parameter("resolution", 0.05)), + occupancy_grid_map_threshold_(kPixelMax * declare_parameter("occupied_thresh", 0.65)), + map_yaml_path_(declare_parameter("map_yaml_path", "")), + map_png_path_(map_yaml_path_.substr(0, map_yaml_path_.find_last_of( + "/\\")) + "/" + declare_parameter("image", "")), + map_origin_(declare_parameter>("origin", {0.0, 0.0, 0.0})), + max_points_(declare_parameter("max_points", 20000)), + use_gxf_map_convention_(declare_parameter("use_gxf_map_convention", false)), + robot_radius_(declare_parameter("robot_radius", 0.25)), + max_beam_error_(declare_parameter("max_beam_error", 0.5)), + num_beams_gpu_(declare_parameter("num_beams_gpu", 512)), + batch_size_(declare_parameter("batch_size", 512)), + out_of_range_threshold_(declare_parameter("out_of_range_threshold", 100.0)), + invalid_range_threshold_(declare_parameter("invalid_range_threshold", 0.0)), + min_scan_fov_degrees_(declare_parameter("min_scan_fov_degrees", 270.0)), + use_closest_beam_(declare_parameter("use_closest_beam", true)), + min_output_error_(declare_parameter("min_output_error", 0.22)), + max_output_error_(declare_parameter("max_output_error", 0.35)) +{ + RCLCPP_DEBUG(get_logger(), "[OccupancyGridLocalizerNode] Constructor"); + + // Initialize tf buffer + tf_buffer_ = + std::make_unique(this->get_clock()); + tf_listener_ = + std::make_shared(*tf_buffer_); + + // Input Flatscan msg callback to populate atlas with transform wrt to robot frame + config_map_[INPUT_COMPONENT_KEY_FLATSCAN].callback = + std::bind( + &OccupancyGridLocalizerNode::FlatScanNitrosSubCallback, this, + std::placeholders::_1, std::placeholders::_2); + + // Pose3DCov callback to convert map frame conventions from Isaac to ROS + if (!use_gxf_map_convention_) { + config_map_[OUTPUT_COMPONENT_KEY_LOC_RESULT].callback = + std::bind( + &OccupancyGridLocalizerNode::LocResultNitrosSubCallback, this, + std::placeholders::_1, std::placeholders::_2); + } + + // Exit with error if map is not specified + if (!fs::exists(map_png_path_)) { + RCLCPP_ERROR( + this->get_logger(), "Could not find map at %s. Exiting.", + map_png_path_.c_str()); + throw std::runtime_error("Parameter parsing failure."); + } + + GePngImageHeight(map_png_path_, map_png_height_); + + registerSupportedType(); + registerSupportedType(); + + setFrameIdSource( + OUTPUT_FRAME_ID_MAP_KEY, + declare_parameter("loc_result_frame", "map")); + grid_search_localize_service_server_ = + this->create_service( + "trigger_grid_search_localization", + std::bind( + &OccupancyGridLocalizerNode::GridSearchLocalizationCallback, this, + std::placeholders::_1, std::placeholders::_2)); + flat_scan_subscriber_ = + this->create_subscription( + "flatscan", 10, + std::bind( + &OccupancyGridLocalizerNode::FlatScanCallback, this, + std::placeholders::_1)); + flat_scan_publisher_ = + this->create_publisher( + INPUT_TOPIC_NAME_FLATSCAN, 10); + startNitrosNode(); +} + +void OccupancyGridLocalizerNode::GePngImageHeight( + const std::string & file_path, + unsigned int & height) +{ + // The width is a 4-byte integer starting at offset 16 in the file. + // See below link for more details + // https://stackoverflow.com/questions/5354459/ + // c-how-to-get-the-image-size-of-a-png-file-in-directory/5354657#5354657 + unsigned char buf[8]; + + std::ifstream in(file_path); + in.seekg(16); + in.read(reinterpret_cast(&buf), 8); + + height = (buf[4] << 24) + (buf[5] << 16) + (buf[6] << 8) + (buf[7] << 0); +} + +void OccupancyGridLocalizerNode::GridSearchLocalizationCallback( + const std::shared_ptr, + std::shared_ptr) +{ + if (last_flat_scan_) { + flat_scan_publisher_->publish(**last_flat_scan_); + } else { + RCLCPP_WARN( + get_logger(), + "[OccupancyGridLocalizerNode] No Flatscan message received! Cannot localize."); + } +} + +void OccupancyGridLocalizerNode::FlatScanCallback( + const std::shared_ptr flat_scan) +{ + last_flat_scan_ = flat_scan; +} + +void OccupancyGridLocalizerNode::FlatScanNitrosSubCallback( + const gxf_context_t context, + nitros::NitrosTypeBase & msg) +{ + auto msg_entity = nvidia::gxf::Entity::Shared(context, msg.handle); + // Get sensor pose id. It should have been populated by the type adapter + // or source gxf codelet + auto maybe_pose_frame_uid = msg_entity->get(kNamePoseFrameUid); + if (!maybe_pose_frame_uid) { + std::stringstream error_msg; + error_msg << + "[OccupancyGridLocalizerNode] Could not get PoseFrameUid in gxf message: " << + GxfResultStr(maybe_pose_frame_uid.error()); + RCLCPP_ERROR( + get_logger(), error_msg.str().c_str()); + throw std::runtime_error(error_msg.str().c_str()); + } + auto pose_frame_uid = maybe_pose_frame_uid.value(); + + // Get pointer to posetree component + gxf_uid_t cid; + nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid( + kPoseTreeEntityName, kPoseTreeComponentName, kPoseTreeComponentTypeName, cid); + auto maybe_pose_tree_handle = + nvidia::gxf::Handle::Create(context, cid); + if (!maybe_pose_tree_handle) { + std::stringstream error_msg; + error_msg << + "[OccupancyGridLocalizerNode] Failed to get pose tree's handle: " << + GxfResultStr(maybe_pose_tree_handle.error()); + RCLCPP_ERROR( + get_logger(), error_msg.str().c_str()); + throw std::runtime_error(error_msg.str().c_str()); + } + auto pose_tree_handle = maybe_pose_tree_handle.value(); + + // Get frame name from atlas. It should have been populated by the type adapter + // or source gxf codelet + auto maybe_frame_name = pose_tree_handle->getFrameName(pose_frame_uid->uid); + if (!maybe_frame_name) { + std::stringstream error_msg; + error_msg << + "[OccupancyGridLocalizerNode] Cannot not find Pose Tree Frame"; + RCLCPP_ERROR( + get_logger(), error_msg.str().c_str()); + throw std::runtime_error(error_msg.str().c_str()); + } + auto frame_name = maybe_frame_name.value(); + + // Look up the last transformation between base_link and FlatScan frame_id + // Wil + geometry_msgs::msg::TransformStamped baselink_to_lidar_transform; + try { + baselink_to_lidar_transform = tf_buffer_->lookupTransform( + kBaseLinkFrameName, frame_name, + tf2::TimePointZero); + } catch (const tf2::TransformException & ex) { + RCLCPP_INFO( + this->get_logger(), "Could not transform %s to %s: %s. Using Identity transform", + kBaseLinkFrameName, frame_name, ex.what()); + } + + // Get pointer to posetree component + nvidia::isaac_ros::nitros::GetTypeAdapterNitrosContext().getCid( + kPoseTreeEntityName, kRobotFrameComponentName, kRobotFrameComponentTypeName, cid); + auto maybe_robot_frame_handle = + nvidia::gxf::Handle::Create(context, cid); + if (!maybe_robot_frame_handle) { + std::stringstream error_msg; + error_msg << + "[OccupancyGridLocalizerNode] Failed to get pose tree's handle: " << + GxfResultStr(maybe_robot_frame_handle.error()); + RCLCPP_ERROR( + get_logger(), error_msg.str().c_str()); + throw std::runtime_error(error_msg.str().c_str()); + } + auto robot_frame_handle = maybe_robot_frame_handle.value(); + auto robot_frame_uid = robot_frame_handle->frame_uid(); + + // Populate atlas with transformation between base_link + // and FlatScan frame_id + pose_tree_handle->set( + robot_frame_uid, pose_frame_uid->uid, 0.0, + ::isaac::Pose3d{ + ::isaac::SO3d::FromQuaternion( + ::isaac::Quaterniond{ + baselink_to_lidar_transform.transform.rotation.w, + baselink_to_lidar_transform.transform.rotation.x, + baselink_to_lidar_transform.transform.rotation.y, + baselink_to_lidar_transform.transform.rotation.z}), + ::isaac::Vector3d( + baselink_to_lidar_transform.transform.translation.x, + baselink_to_lidar_transform.transform.translation.y, + baselink_to_lidar_transform.transform.translation.z)} + ); +} + +// The below callback is used to convert the result from the Isaac +// to ROS map origin conventions. +void OccupancyGridLocalizerNode::LocResultNitrosSubCallback( + const gxf_context_t context, + nitros::NitrosTypeBase & msg) +{ + auto msg_entity = nvidia::gxf::Entity::Shared(context, msg.handle); + // Get pose + auto maybe_pose = msg_entity->get<::isaac::Pose3d>(kPoseName); + if (!maybe_pose) { + std::stringstream error_msg; + error_msg << + "[OccupancyGridLocalizerNode] Could not get Pose3d in gxf message: " << + GxfResultStr(maybe_pose.error()); + RCLCPP_ERROR( + get_logger(), error_msg.str().c_str()); + throw std::runtime_error(error_msg.str().c_str()); + } + auto pose_handle = maybe_pose.value(); + + // ROS map_server convention is that the origin is the bottom left corner, + // with the X-axis in pointing right and the Y-axis pointing to the up. + // Isaac convention is that the origin is the top left corner, + // with the X-axis in pointing down and the Y-axis pointing to the right. + // Hence the transformation matrix from the Isaac map frame to the + // ROS map frame is as shown below + auto ros_pose_isaac_transform = ::isaac::Pose3d{ + ::isaac::SO3d::FromQuaternion( + ::isaac::Quaterniond{0.7071068, 0.0, 0.0, -0.7071068}), + ::isaac::Vector3d(0.0, map_png_height_ * cell_size_, 0.0) + }; + + auto ros_map_origin_transform = + ::isaac::Pose3d::FromPose2XY( + {::isaac::SO2d::FromAngle( + map_origin_[2]), {map_origin_[0], map_origin_[1]}}); + + // The ros_pose_isaac_transform transformation matrix is + // applied so that the published output conforms to ROS map conventions + *pose_handle = ros_map_origin_transform * ros_pose_isaac_transform * (*pose_handle); +} + +void OccupancyGridLocalizerNode::postLoadGraphCallback() +{ + RCLCPP_DEBUG(get_logger(), "[OccupancyGridLocalizerNode] postLoadGraphCallback()."); + getNitrosContext().setParameterFloat64( + "binary_occupancy_map_loader", "nvidia::isaac::OccupancyGridMap", "cell_size", cell_size_); + getNitrosContext().setParameterInt32( + "binary_occupancy_map_loader", "nvidia::isaac::OccupancyGridMap", "threshold", + occupancy_grid_map_threshold_); + getNitrosContext().setParameterFloat64( + "global_localization", "nvidia::isaac::localization::GridSearchLocalizer", "robot_radius", + robot_radius_); + getNitrosContext().setParameterFloat64( + "global_localization", "nvidia::isaac::localization::GridSearchLocalizer", "max_beam_error", + max_beam_error_); + getNitrosContext().setParameterInt32( + "global_localization", "nvidia::isaac::localization::GridSearchLocalizer", "num_beams_gpu", + num_beams_gpu_); + getNitrosContext().setParameterInt32( + "global_localization", "nvidia::isaac::localization::GridSearchLocalizer", "batch_size", + batch_size_); + getNitrosContext().setParameterFloat64( + "global_localization", "nvidia::isaac::localization::GridSearchLocalizer", + "out_of_range_threshold", + out_of_range_threshold_); + getNitrosContext().setParameterFloat64( + "global_localization", "nvidia::isaac::localization::GridSearchLocalizer", + "invalid_range_threshold", + invalid_range_threshold_); + getNitrosContext().setParameterFloat64( + "global_localization", "nvidia::isaac::localization::GridSearchLocalizer", + "min_scan_fov_degrees", + min_scan_fov_degrees_); + getNitrosContext().setParameterBool( + "global_localization", "nvidia::isaac::localization::GridSearchLocalizer", + "use_closest_beam", + use_closest_beam_); + getNitrosContext().setParameterFloat64( + "global_localization", "nvidia::isaac::localization::GridSearchLocalizer", + "min_output_error", + min_output_error_); + getNitrosContext().setParameterFloat64( + "global_localization", "nvidia::isaac::localization::GridSearchLocalizer", + "max_output_error", + max_output_error_); + getNitrosContext().setParameterStr( + "binary_occupancy_map_loader", "nvidia::isaac::ImageLoader", "filename", map_png_path_); + // decide how much memory to allocate based on how much memory used in + // FLatScan Message parser source code + getNitrosContext().setParameterUInt64( + "flatscan_beams_tensor_copier", "nvidia::gxf::BlockMemoryPool", + "block_size", + kFlatScanGxfMsgSize * kMsgBufferSize * sizeof(double) * max_points_); +} + +OccupancyGridLocalizerNode::~OccupancyGridLocalizerNode() {} + +} // namespace occupancy_grid_localizer +} // namespace isaac_ros +} // namespace nvidia + +RCLCPP_COMPONENTS_REGISTER_NODE( + nvidia::isaac_ros::occupancy_grid_localizer::OccupancyGridLocalizerNode) diff --git a/isaac_ros_occupancy_grid_localizer/test/isaac_ros_occupancy_grid_localizer_pol_test.py b/isaac_ros_occupancy_grid_localizer/test/isaac_ros_occupancy_grid_localizer_pol_test.py new file mode 100644 index 0000000..a7b7fe8 --- /dev/null +++ b/isaac_ros_occupancy_grid_localizer/test/isaac_ros_occupancy_grid_localizer_pol_test.py @@ -0,0 +1,158 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +import os +import pathlib +import time + +from ament_index_python.packages import get_package_share_directory + +from geometry_msgs.msg import PoseWithCovarianceStamped +from isaac_ros_test import IsaacROSBaseTest +from launch.actions import ExecuteProcess +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +import pytest +import rclpy +from std_srvs.srv import Empty + +TEST_POS_TOLERANCE = 0.01 +TEST_QUAT_TOLERANCE = 0.001 +TEST_COV_TOLERANCE = 0.001 + + +@pytest.mark.rostest +def generate_test_description(): + """Generate launch description with all ROS 2 nodes for testing.""" + map_yaml_path = os.path.join( + get_package_share_directory('isaac_ros_occupancy_grid_localizer'), 'maps', 'map.yaml') + + occupancy_grid_localizer_node = ComposableNode( + package='isaac_ros_occupancy_grid_localizer', + plugin='nvidia::isaac_ros::occupancy_grid_localizer::OccupancyGridLocalizerNode', + name='occupancy_grid_localizer', + namespace=IsaacROSOccupancyGridLocalizerPOLTest.generate_namespace(), + parameters=[map_yaml_path, { + 'loc_result_frame': 'map', + 'map_yaml_path': map_yaml_path, + }] + ) + + rosbag_play = ExecuteProcess( + cmd=['ros2', 'bag', 'play', os.path.dirname(__file__) + + '/../data/rosbags/flatscan', '-l', + '--remap', + '/flatscan:=' + + IsaacROSOccupancyGridLocalizerPOLTest.generate_namespace() + + '/flatscan'], + output='screen' + ) + + occupancy_grid_localizer_container = ComposableNodeContainer( + package='rclcpp_components', + name='occupancy_grid_localizer_container', + namespace='', + executable='component_container_mt', + composable_node_descriptions=[ + occupancy_grid_localizer_node, + ], + output='screen' + ) + return IsaacROSOccupancyGridLocalizerPOLTest.generate_test_description([ + occupancy_grid_localizer_container, + rosbag_play + ]) + + +class IsaacROSOccupancyGridLocalizerPOLTest(IsaacROSBaseTest): + """Test for Isaac ROS OccupancyGridLocalizer Proof of Life.""" + + filepath = pathlib.Path(os.path.dirname(__file__)) + + def test_occupancy_grid_localizer_pipeline(self) -> None: + """Expect the pipeline to produce Intialpose data from scan and map.""" + self.generate_namespace_lookup( + ['localization_result']) + + received_messages = {} + localization_result_sub, = self.create_logging_subscribers( + [('localization_result', PoseWithCovarianceStamped)], received_messages, + accept_multiple_messages=True) + # Create service client + self.cli = self.node.create_client( + Empty, '/isaac_ros_test/trigger_grid_search_localization') + self.req = Empty.Request() + # Check if the a service is available + while not self.cli.wait_for_service(timeout_sec=1.0): + self.node.get_logger().info('service not available, waiting again...') + try: + # Wait at most TIMEOUT seconds for subscriber to respond + TIMEOUT = 20 + end_time = time.time() + TIMEOUT + + done = False + while time.time() < end_time: + self.future = self.cli.call_async(self.req) + rclpy.spin_until_future_complete(self.node, self.future) + # If we have received exactly one message on the output topic, break + if len(received_messages['localization_result']) > 0: + done = True + break + + self.assertTrue( + done, "Didn't receive output on localization_result topic!") + + loc_result_actual = received_messages['localization_result'][0] + self.assertAlmostEqual(loc_result_actual.pose.pose.position.x, + 33.5, None, + 'received position X coordinate is not within tolerance', + TEST_POS_TOLERANCE) + self.assertAlmostEqual(loc_result_actual.pose.pose.position.y, + 7.75, None, + 'received position Y coordinate is not within tolerance', + TEST_POS_TOLERANCE) + self.assertAlmostEqual(loc_result_actual.pose.pose.position.z, + 0.0, None, + 'received position Z coordinate is not within tolerance', + TEST_POS_TOLERANCE) + self.assertAlmostEqual(loc_result_actual.pose.pose.orientation.x, + 0.0, None, + 'received quaternion X is not within tolerance', + TEST_QUAT_TOLERANCE) + self.assertAlmostEqual(loc_result_actual.pose.pose.orientation.y, + 0.0, None, + 'received quaternion Y is not within tolerance', + TEST_QUAT_TOLERANCE) + self.assertAlmostEqual(loc_result_actual.pose.pose.orientation.z, + -0.56573, None, + 'received quaternion Z is not within tolerance', + TEST_QUAT_TOLERANCE) + self.assertAlmostEqual(loc_result_actual.pose.pose.orientation.w, + 0.824589, None, + 'received quaternion W is not within tolerance', + TEST_QUAT_TOLERANCE) + for i in range(36): + self.assertAlmostEqual(loc_result_actual.pose.covariance[i], + 0.0, None, + 'received covariance is not within tolerance', + TEST_COV_TOLERANCE) + + finally: + pass + self.assertTrue(self.node.destroy_subscription( + localization_result_sub)) diff --git a/isaac_ros_pointcloud_utils/CMakeLists.txt b/isaac_ros_pointcloud_utils/CMakeLists.txt new file mode 100644 index 0000000..f55c0af --- /dev/null +++ b/isaac_ros_pointcloud_utils/CMakeLists.txt @@ -0,0 +1,56 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.5) +project(isaac_ros_pointcloud_utils LANGUAGES C CXX) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +# Eigen +find_package(Eigen3 REQUIRED) +find_package(Threads REQUIRED) +include_directories(${EIGEN3_INCLUDE_DIR}) + +# pointcloud_to_flatscan +ament_auto_add_library(pointcloud_to_flatscan SHARED src/pointcloud_to_flatscan_node.cpp) +rclcpp_components_register_nodes(pointcloud_to_flatscan "nvidia::isaac_ros::pointcloud_utils::PointCloudToFlatScanNode") +set(node_plugins "${node_plugins}nvidia::isaac_ros::pointcloud_utils::PointCloudToFlatScanNode;$\n") + +# flatscan_to_laserscan_node +ament_auto_add_library(flatscan_to_laserscan_node SHARED src/flatscan_to_laserscan_node.cpp) +rclcpp_components_register_nodes(flatscan_to_laserscan_node "nvidia::isaac_ros::pointcloud_utils::FlatScantoLaserScanNode") +set(node_plugins "${node_plugins}nvidia::isaac_ros::pointcloud_utils::FlatScantoLaserScanNode;$\n") + +# laserscan_to_flatscan_node +ament_auto_add_library(laserscan_to_flatscan_node SHARED src/laserscan_to_flatscan_node.cpp) +rclcpp_components_register_nodes(laserscan_to_flatscan_node "nvidia::isaac_ros::pointcloud_utils::LaserScantoFlatScanNode") +set(node_plugins "${node_plugins}nvidia::isaac_ros::pointcloud_utils::LaserScantoFlatScanNode;$\n") + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(launch_testing_ament_cmake REQUIRED) + add_launch_test(test/isaac_ros_pointcloud_to_flatscan_pol_test.py) +endif() + +ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/isaac_ros_pointcloud_utils/config/pointcloud_to_flatscan_node.yaml b/isaac_ros_pointcloud_utils/config/pointcloud_to_flatscan_node.yaml new file mode 100644 index 0000000..c46a818 --- /dev/null +++ b/isaac_ros_pointcloud_utils/config/pointcloud_to_flatscan_node.yaml @@ -0,0 +1,96 @@ +%YAML 1.2 +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 +--- +############################### +# Flatten 3D lidar data to 2D # +############################### +name: pointcloud_to_flatscan +components: +- name: tx_flatscan + type: nvidia::gxf::DoubleBufferTransmitter +- type: nvidia::gxf::DownstreamReceptiveSchedulingTerm + parameters: + transmitter: tx_flatscan + min_size: 1 +- name: rx_point_cloud + type: nvidia::gxf::DoubleBufferReceiver +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: rx_point_cloud + min_size: 1 +- name: allocator + type: nvidia::gxf::BlockMemoryPool + parameters: + storage_type: 1 + block_size: 19200000 + num_blocks: 120 +- name: pointcloud_to_flatscan + type: nvidia::isaac_ros::point_cloud::PointCloudToFlatscan + parameters: + allocator: allocator + rx_point_cloud: rx_point_cloud + tx_flatscan: tx_flatscan + threshold_x_axis: false + threshold_y_axis: false + min_x: -0.0 + max_x: 4.0 + min_y: -0.0 + max_y: 4.0 + min_z: -0.1 + max_z: 0.1 + max_points: 691200 +--- +########################### +# Vault for ROS Publisher # +########################### +name: vault +components: +- name: signal + type: nvidia::gxf::DoubleBufferReceiver + parameters: + capacity: 1 + policy: 0 +- type: nvidia::gxf::MessageAvailableSchedulingTerm + parameters: + receiver: signal + min_size: 1 +- name: vault + type: nvidia::gxf::Vault + parameters: + source: signal + max_waiting_count: 1 + drop_waiting: false +--- +components: +- name: edge0 + type: nvidia::gxf::Connection + parameters: + source: pointcloud_to_flatscan/tx_flatscan + target: vault/signal +--- +components: +- name: clock + type: nvidia::gxf::RealtimeClock +- type: nvidia::gxf::MultiThreadScheduler + parameters: + clock: clock + worker_thread_number: 8 + stop_on_deadlock: false +- type: nvidia::gxf::JobStatistics + parameters: + clock: clock diff --git a/isaac_ros_pointcloud_utils/data/rosbags/pointcloud/metadata.yaml b/isaac_ros_pointcloud_utils/data/rosbags/pointcloud/metadata.yaml new file mode 100644 index 0000000..33be42b --- /dev/null +++ b/isaac_ros_pointcloud_utils/data/rosbags/pointcloud/metadata.yaml @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:99d691b1a55781b1b10138e768655bbdfb1527e618b316bf5bb179eb5bae998c +size 983 diff --git a/isaac_ros_pointcloud_utils/data/rosbags/pointcloud/pointcloud_0.db3 b/isaac_ros_pointcloud_utils/data/rosbags/pointcloud/pointcloud_0.db3 new file mode 100644 index 0000000..c508d2c --- /dev/null +++ b/isaac_ros_pointcloud_utils/data/rosbags/pointcloud/pointcloud_0.db3 @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:806b3e2eee41eb350728b0b0b6918663db1714e04e969cc1d7e35bd85dfd7853 +size 16883712 diff --git a/isaac_ros_pointcloud_utils/include/isaac_ros_pointcloud_utils/flatscan_to_laserscan_node.hpp b/isaac_ros_pointcloud_utils/include/isaac_ros_pointcloud_utils/flatscan_to_laserscan_node.hpp new file mode 100644 index 0000000..3243afd --- /dev/null +++ b/isaac_ros_pointcloud_utils/include/isaac_ros_pointcloud_utils/flatscan_to_laserscan_node.hpp @@ -0,0 +1,57 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#ifndef ISAAC_ROS_POINTCLOUD_UTILS__FLATSCAN_TO_LASERSCAN_NODE_HPP_ +#define ISAAC_ROS_POINTCLOUD_UTILS__FLATSCAN_TO_LASERSCAN_NODE_HPP_ + +#include "isaac_ros_pointcloud_interfaces/msg/flat_scan.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" + + +namespace nvidia +{ +namespace isaac_ros +{ +namespace pointcloud_utils +{ + +class FlatScantoLaserScanNode : public rclcpp::Node +{ +public: + explicit FlatScantoLaserScanNode(const rclcpp::NodeOptions options = rclcpp::NodeOptions()); + + ~FlatScantoLaserScanNode(); + +private: + const double angle_min_; // Units: radiands + const double angle_max_; // Units: radiands + const double angle_increment_; // Units: radiands + const double time_increment_; // Units: seconds + const double max_range_fallback_; // Units: meters + + rclcpp::Subscription::SharedPtr flatscan_sub_; + rclcpp::Publisher::SharedPtr laserscan_pub_; + + void FlatScanCallback(const isaac_ros_pointcloud_interfaces::msg::FlatScan::SharedPtr msg); +}; + +} // namespace pointcloud_utils +} // namespace isaac_ros +} // namespace nvidia + +#endif // ISAAC_ROS_POINTCLOUD_UTILS__FLATSCAN_TO_LASERSCAN_NODE_HPP_ diff --git a/isaac_ros_pointcloud_utils/include/isaac_ros_pointcloud_utils/laserscan_to_flatscan_node.hpp b/isaac_ros_pointcloud_utils/include/isaac_ros_pointcloud_utils/laserscan_to_flatscan_node.hpp new file mode 100644 index 0000000..5b86d19 --- /dev/null +++ b/isaac_ros_pointcloud_utils/include/isaac_ros_pointcloud_utils/laserscan_to_flatscan_node.hpp @@ -0,0 +1,51 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#ifndef ISAAC_ROS_POINTCLOUD_UTILS__LASERSCAN_TO_FLATSCAN_NODE_HPP_ +#define ISAAC_ROS_POINTCLOUD_UTILS__LASERSCAN_TO_FLATSCAN_NODE_HPP_ + +#include "isaac_ros_pointcloud_interfaces/msg/flat_scan.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" + + +namespace nvidia +{ +namespace isaac_ros +{ +namespace pointcloud_utils +{ + +class LaserScantoFlatScanNode : public rclcpp::Node +{ +public: + explicit LaserScantoFlatScanNode(const rclcpp::NodeOptions options = rclcpp::NodeOptions()); + + ~LaserScantoFlatScanNode(); + +private: + rclcpp::Subscription::SharedPtr laserscan_sub_; + rclcpp::Publisher::SharedPtr flatscan_pub_; + + void LaserScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg); +}; + +} // namespace pointcloud_utils +} // namespace isaac_ros +} // namespace nvidia + +#endif // ISAAC_ROS_POINTCLOUD_UTILS__LASERSCAN_TO_FLATSCAN_NODE_HPP_ diff --git a/isaac_ros_pointcloud_utils/include/isaac_ros_pointcloud_utils/pointcloud_to_flatscan_node.hpp b/isaac_ros_pointcloud_utils/include/isaac_ros_pointcloud_utils/pointcloud_to_flatscan_node.hpp new file mode 100644 index 0000000..76ae209 --- /dev/null +++ b/isaac_ros_pointcloud_utils/include/isaac_ros_pointcloud_utils/pointcloud_to_flatscan_node.hpp @@ -0,0 +1,67 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#ifndef ISAAC_ROS_POINTCLOUD_UTILS__POINTCLOUD_TO_FLATSCAN_NODE_HPP_ +#define ISAAC_ROS_POINTCLOUD_UTILS__POINTCLOUD_TO_FLATSCAN_NODE_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "isaac_ros_nitros/nitros_node.hpp" +#include "isaac_ros_nitros/types/nitros_type_base.hpp" + +namespace nvidia +{ +namespace isaac_ros +{ +namespace pointcloud_utils +{ + +class PointCloudToFlatScanNode : public nitros::NitrosNode +{ +public: + explicit PointCloudToFlatScanNode(const rclcpp::NodeOptions &); + + // The callback to be implemented by users for any required initialization + void postLoadGraphCallback() override; + + ~PointCloudToFlatScanNode(); + + PointCloudToFlatScanNode(const PointCloudToFlatScanNode &) = delete; + + PointCloudToFlatScanNode & operator=(const PointCloudToFlatScanNode &) = delete; + +private: + const double min_x_; + const double max_x_; + const double min_y_; + const double max_y_; + const double min_z_; + const double max_z_; + const int max_points_; + const bool threshold_x_axis_; + const bool threshold_y_axis_; +}; + +} // namespace pointcloud_utils +} // namespace isaac_ros +} // namespace nvidia + +#endif // ISAAC_ROS_POINTCLOUD_UTILS__POINTCLOUD_TO_FLATSCAN_NODE_HPP_ diff --git a/isaac_ros_pointcloud_utils/launch/isaac_ros_flatscan_to_laserscan.launch.py b/isaac_ros_pointcloud_utils/launch/isaac_ros_flatscan_to_laserscan.launch.py new file mode 100644 index 0000000..fa6eb6b --- /dev/null +++ b/isaac_ros_pointcloud_utils/launch/isaac_ros_flatscan_to_laserscan.launch.py @@ -0,0 +1,48 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +# Import math Library +import math + +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + flatscan_to_laserscan_node = ComposableNode( + package='isaac_ros_pointcloud_utils', + plugin='nvidia::isaac_ros::pointcloud_utils::FlatScantoLaserScanNode', + name='flatscan_to_laserscan', + parameters=[{'angle_min': 0.0, + 'angle_max': 2*math.pi, + 'angle_increment': math.pi/180, # 0.0174533 rad => 1deg + 'time_increment': 0.0001, + 'max_range_fallback': 200.0}]) + + flatscan_to_laserscan_container = ComposableNodeContainer( + package='rclcpp_components', + name='flatscan_to_laserscan_container', + namespace='', + executable='component_container_mt', + composable_node_descriptions=[ + flatscan_to_laserscan_node, + ], + output='screen' + ) + + return launch.LaunchDescription([flatscan_to_laserscan_container]) diff --git a/isaac_ros_pointcloud_utils/launch/isaac_ros_laserscan_to_flatscan.launch.py b/isaac_ros_pointcloud_utils/launch/isaac_ros_laserscan_to_flatscan.launch.py new file mode 100644 index 0000000..820651f --- /dev/null +++ b/isaac_ros_pointcloud_utils/launch/isaac_ros_laserscan_to_flatscan.launch.py @@ -0,0 +1,40 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + laserscan_to_flatscan_node = ComposableNode( + package='isaac_ros_pointcloud_utils', + plugin='nvidia::isaac_ros::pointcloud_utils::LaserScantoFlatScanNode', + name='laserscan_to_flatscan') + + laserscan_to_flatscan_container = ComposableNodeContainer( + package='rclcpp_components', + name='laserscan_to_flatscan_container', + namespace='', + executable='component_container_mt', + composable_node_descriptions=[ + laserscan_to_flatscan_node, + ], + output='screen' + ) + + return launch.LaunchDescription([laserscan_to_flatscan_container]) diff --git a/isaac_ros_pointcloud_utils/launch/isaac_ros_pointcloud_to_flatscan.launch.py b/isaac_ros_pointcloud_utils/launch/isaac_ros_pointcloud_to_flatscan.launch.py new file mode 100644 index 0000000..9709383 --- /dev/null +++ b/isaac_ros_pointcloud_utils/launch/isaac_ros_pointcloud_to_flatscan.launch.py @@ -0,0 +1,41 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +import launch +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + pointcloud_to_flatscan_node = ComposableNode( + package='isaac_ros_pointcloud_utils', + plugin='nvidia::isaac_ros::pointcloud_utils::PointCloudToFlatScanNode', + name='pointcloud_to_flatscan', + parameters=[{'flatscan_frame': 'lidar'}]) + + pointcloud_to_flatscan_container = ComposableNodeContainer( + package='rclcpp_components', + name='pointcloud_to_flatscan_container', + namespace='', + executable='component_container_mt', + composable_node_descriptions=[ + pointcloud_to_flatscan_node, + ], + output='screen' + ) + + return launch.LaunchDescription([pointcloud_to_flatscan_container]) diff --git a/isaac_ros_pointcloud_utils/package.xml b/isaac_ros_pointcloud_utils/package.xml new file mode 100644 index 0000000..2543c7f --- /dev/null +++ b/isaac_ros_pointcloud_utils/package.xml @@ -0,0 +1,50 @@ + + + + + + + isaac_ros_pointcloud_utils + 0.30.0 + Isaac ROS Point Cloud Utilities. + + Ashwin Varghese Kuruttukulam + Apache-2.0 + https://developer.nvidia.com/isaac-ros/ + Ashwin Varghese Kuruttukulam + + ament_cmake_auto + + rclcpp + rclcpp_components + isaac_ros_nitros + isaac_ros_pointcloud_interfaces + isaac_ros_nitros_flat_scan_type + isaac_ros_nitros_point_cloud_type + + isaac_ros_common + + ament_lint_auto + ament_lint_common + isaac_ros_test + + + ament_cmake + + diff --git a/isaac_ros_pointcloud_utils/src/flatscan_to_laserscan_node.cpp b/isaac_ros_pointcloud_utils/src/flatscan_to_laserscan_node.cpp new file mode 100644 index 0000000..6261032 --- /dev/null +++ b/isaac_ros_pointcloud_utils/src/flatscan_to_laserscan_node.cpp @@ -0,0 +1,96 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#include "isaac_ros_pointcloud_utils/flatscan_to_laserscan_node.hpp" + +#include "rclcpp_components/register_node_macro.hpp" + +#include "rclcpp/logger.hpp" + +namespace nvidia +{ +namespace isaac_ros +{ +namespace pointcloud_utils +{ + + +FlatScantoLaserScanNode::FlatScantoLaserScanNode(rclcpp::NodeOptions options) +: Node("flatscan_to_laserscan", options.use_intra_process_comms(true)), + // parameter + angle_min_(declare_parameter("angle_min", 0.0)), + angle_max_(declare_parameter("angle_max", 2 * M_PI)), + angle_increment_(declare_parameter("angle_increment", M_PI / 180)), + time_increment_(declare_parameter("time_increment", 0.0001)), + max_range_fallback_(declare_parameter("max_range_fallback", 200.0)), + // topics + flatscan_sub_(create_subscription( + "/flatscan", 10, std::bind(&FlatScantoLaserScanNode::FlatScanCallback, this, + std::placeholders::_1))), + laserscan_pub_( + create_publisher( + "/scan", rclcpp::QoS(10))) +{} + +void FlatScantoLaserScanNode::FlatScanCallback( + const isaac_ros_pointcloud_interfaces::msg::FlatScan::SharedPtr msg) +{ + auto laser_scan_message = sensor_msgs::msg::LaserScan(); + laser_scan_message.header = msg->header; + laser_scan_message.angle_min = angle_min_; + laser_scan_message.angle_max = angle_max_; + laser_scan_message.angle_increment = angle_increment_; + laser_scan_message.time_increment = time_increment_; + laser_scan_message.scan_time = time_increment_ * msg->ranges.size(); + laser_scan_message.range_min = msg->range_min; + if (msg->range_max != 0.0) { + laser_scan_message.range_max = msg->range_max; + } else { + laser_scan_message.range_max = max_range_fallback_; + } + // determine amount of rays to create + uint32_t ranges_size = std::ceil( + (laser_scan_message.angle_max - laser_scan_message.angle_min) / + laser_scan_message.angle_increment); + laser_scan_message.ranges.assign(ranges_size, laser_scan_message.range_max); + + for (size_t i = 0; i < msg->ranges.size(); i++) { + float range = msg->ranges[i]; + float angle = msg->angles[i]; + // Ensure that angles are within [0,2*M_PI) + if (angle >= 2 * M_PI || angle <= -2 * M_PI) { + angle = fmod(angle, (2 * M_PI)); + } + if (angle < 0) { + angle = angle + 2 * M_PI; + } + int index = (angle - laser_scan_message.angle_min) / laser_scan_message.angle_increment; + if (range < laser_scan_message.ranges[index]) { + laser_scan_message.ranges[index] = range; + } + } + laserscan_pub_->publish(laser_scan_message); +} + + +FlatScantoLaserScanNode::~FlatScantoLaserScanNode() = default; + +} // namespace pointcloud_utils +} // namespace isaac_ros +} // namespace nvidia + +RCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::pointcloud_utils::FlatScantoLaserScanNode) diff --git a/isaac_ros_pointcloud_utils/src/laserscan_to_flatscan_node.cpp b/isaac_ros_pointcloud_utils/src/laserscan_to_flatscan_node.cpp new file mode 100644 index 0000000..724e6be --- /dev/null +++ b/isaac_ros_pointcloud_utils/src/laserscan_to_flatscan_node.cpp @@ -0,0 +1,86 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#include "isaac_ros_pointcloud_utils/laserscan_to_flatscan_node.hpp" + +#include "rclcpp_components/register_node_macro.hpp" +#include "rclcpp/logger.hpp" + +namespace +{ +constexpr int LASERSCAN_N_DIFF_TOL_ = 1; +} + +namespace nvidia +{ +namespace isaac_ros +{ +namespace pointcloud_utils +{ + + +LaserScantoFlatScanNode::LaserScantoFlatScanNode(rclcpp::NodeOptions options) +: Node("laserscan_to_flatscan", options.use_intra_process_comms(true)), + // topics + laserscan_sub_(create_subscription( + "/scan", 10, std::bind(&LaserScantoFlatScanNode::LaserScanCallback, + this, std::placeholders::_1))), + flatscan_pub_( + create_publisher( + "/flatscan", rclcpp::QoS(10))) +{ +} + +void LaserScantoFlatScanNode::LaserScanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg) +{ + auto flatscan_message = isaac_ros_pointcloud_interfaces::msg::FlatScan(); + flatscan_message.header = msg->header; + flatscan_message.range_min = msg->range_min; + flatscan_message.range_max = msg->range_max; + + bool copy_intensities = (msg->ranges.size() == msg->intensities.size()) ? true : false; + float angle = msg->angle_min; + int expected_ranges_size = std::ceil( + (msg->angle_max - msg->angle_min) / + msg->angle_increment); + if (std::abs(expected_ranges_size - static_cast(msg->ranges.size())) > + LASERSCAN_N_DIFF_TOL_) + { + RCLCPP_WARN( + this->get_logger(), "The angle specs and range array size does not match: %u, %lu", + expected_ranges_size, + msg->ranges.size()); + } + for (size_t i = 0; i < msg->ranges.size(); i++) { + flatscan_message.ranges.push_back(msg->ranges[i]); + flatscan_message.angles.push_back(angle); + if (copy_intensities) { + flatscan_message.intensities.push_back(msg->intensities[i]); + } + angle += msg->angle_increment; + } + flatscan_pub_->publish(flatscan_message); +} + + +LaserScantoFlatScanNode::~LaserScantoFlatScanNode() = default; + +} // namespace pointcloud_utils +} // namespace isaac_ros +} // namespace nvidia + +RCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::pointcloud_utils::LaserScantoFlatScanNode) diff --git a/isaac_ros_pointcloud_utils/src/pointcloud_to_flatscan_node.cpp b/isaac_ros_pointcloud_utils/src/pointcloud_to_flatscan_node.cpp new file mode 100644 index 0000000..24aa3fa --- /dev/null +++ b/isaac_ros_pointcloud_utils/src/pointcloud_to_flatscan_node.cpp @@ -0,0 +1,168 @@ +// SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +// Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include + +#include "isaac_ros_pointcloud_utils/pointcloud_to_flatscan_node.hpp" +#include "isaac_ros_nitros_flat_scan_type/nitros_flat_scan.hpp" +#include "isaac_ros_nitros_point_cloud_type/nitros_point_cloud.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-parameter" +#pragma GCC diagnostic ignored "-Wmissing-field-initializers" +#pragma GCC diagnostic ignored "-Wpedantic" +#include "gxf/std/timestamp.hpp" +#pragma GCC diagnostic pop + +namespace +{ +// number of components in PointCloud gxf msg +constexpr int kPointCloudGxfMsgSize = 4; +constexpr int kMsgBufferSize = 5; +} + +namespace nvidia +{ +namespace isaac_ros +{ +namespace pointcloud_utils +{ + +using nvidia::gxf::optimizer::GraphIOGroupSupportedDataTypesInfoList; + +constexpr char INPUT_COMPONENT_KEY_POINTCLOUD[] = "pointcloud_to_flatscan/rx_point_cloud"; +constexpr char INPUT_DEFAULT_TENSOR_FORMAT_POINTCLOUD[] = "nitros_point_cloud"; +constexpr char INPUT_TOPIC_NAME_POINTCLOUD[] = "pointcloud"; + +constexpr char OUTPUT_COMPONENT_KEY_FLATSCAN[] = "vault/vault"; +constexpr char OUTPUT_DEFAULT_TENSOR_FORMAT_FLATSCAN[] = "nitros_flat_scan"; +constexpr char OUTPUT_TOPIC_NAME_FLATSCAN[] = "flatscan"; + +constexpr char APP_YAML_FILENAME[] = "config/pointcloud_to_flatscan_node.yaml"; +constexpr char PACKAGE_NAME[] = "isaac_ros_pointcloud_utils"; + +const std::vector> EXTENSIONS = { + {"isaac_ros_gxf", "gxf/lib/libgxf_point_cloud.so"} +}; +const std::vector PRESET_EXTENSION_SPEC_NAMES = { + "isaac_ros_pointcloud_utils" +}; +const std::vector EXTENSION_SPEC_FILENAMES = {}; +const std::vector GENERATOR_RULE_FILENAMES = {}; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wpedantic" + +const nitros::NitrosPublisherSubscriberConfigMap CONFIG_MAP = { + {INPUT_COMPONENT_KEY_POINTCLOUD, + { + .type = nitros::NitrosPublisherSubscriberType::NEGOTIATED, + .qos = rclcpp::QoS(10), + .compatible_data_format = INPUT_DEFAULT_TENSOR_FORMAT_POINTCLOUD, + .topic_name = INPUT_TOPIC_NAME_POINTCLOUD, + } + }, + {OUTPUT_COMPONENT_KEY_FLATSCAN, + { + .type = nitros::NitrosPublisherSubscriberType::NEGOTIATED, + .qos = rclcpp::QoS(10), + .compatible_data_format = OUTPUT_DEFAULT_TENSOR_FORMAT_FLATSCAN, + .topic_name = OUTPUT_TOPIC_NAME_FLATSCAN, + } + } +}; +#pragma GCC diagnostic pop + +PointCloudToFlatScanNode::PointCloudToFlatScanNode(const rclcpp::NodeOptions & options) +: nitros::NitrosNode(options, + APP_YAML_FILENAME, + CONFIG_MAP, + PRESET_EXTENSION_SPEC_NAMES, + EXTENSION_SPEC_FILENAMES, + GENERATOR_RULE_FILENAMES, + EXTENSIONS, + PACKAGE_NAME), + min_x_(declare_parameter("min_x", -1.0)), + max_x_(declare_parameter("max_x", 1.0)), + min_y_(declare_parameter("min_y", -1.0)), + max_y_(declare_parameter("max_y", 1.0)), + min_z_(declare_parameter("min_z", -0.1)), + max_z_(declare_parameter("max_z", 0.1)), + max_points_(declare_parameter("max_points", 150000)), + threshold_x_axis_(declare_parameter("threshold_x_axis", false)), + threshold_y_axis_(declare_parameter("threshold_y_axis", false)) +{ + RCLCPP_DEBUG(get_logger(), "[PointCloudToFlatScanNode] Constructor"); + + registerSupportedType(); + registerSupportedType(); + + startNitrosNode(); +} + +void PointCloudToFlatScanNode::postLoadGraphCallback() +{ + RCLCPP_DEBUG(get_logger(), "[PointCloudToFlatScanNode] postLoadGraphCallback()."); + getNitrosContext().setParameterFloat64( + "pointcloud_to_flatscan", "nvidia::isaac_ros::point_cloud::PointCloudToFlatscan", "min_x", + min_x_); + getNitrosContext().setParameterFloat64( + "pointcloud_to_flatscan", "nvidia::isaac_ros::point_cloud::PointCloudToFlatscan", "max_x", + max_x_); + getNitrosContext().setParameterFloat64( + "pointcloud_to_flatscan", "nvidia::isaac_ros::point_cloud::PointCloudToFlatscan", "min_y", + min_y_); + getNitrosContext().setParameterFloat64( + "pointcloud_to_flatscan", "nvidia::isaac_ros::point_cloud::PointCloudToFlatscan", "max_y", + max_y_); + getNitrosContext().setParameterFloat64( + "pointcloud_to_flatscan", "nvidia::isaac_ros::point_cloud::PointCloudToFlatscan", "min_z", + min_z_); + getNitrosContext().setParameterFloat64( + "pointcloud_to_flatscan", "nvidia::isaac_ros::point_cloud::PointCloudToFlatscan", "max_z", + max_z_); + getNitrosContext().setParameterInt32( + "pointcloud_to_flatscan", "nvidia::isaac_ros::point_cloud::PointCloudToFlatscan", "max_points", + max_points_); + getNitrosContext().setParameterBool( + "pointcloud_to_flatscan", "nvidia::isaac_ros::point_cloud::PointCloudToFlatscan", + "threshold_x_axis", + threshold_x_axis_); + getNitrosContext().setParameterBool( + "pointcloud_to_flatscan", "nvidia::isaac_ros::point_cloud::PointCloudToFlatscan", + "threshold_y_axis", + threshold_y_axis_); + // decide how much memory to allocate based on how much memory used in + // PointCloudToFlatscan source code + getNitrosContext().setParameterUInt64( + "pointcloud_to_flatscan", "nvidia::gxf::BlockMemoryPool", + "block_size", + kPointCloudGxfMsgSize * kMsgBufferSize * sizeof(float) * max_points_ + sizeof(uint)); +} + +PointCloudToFlatScanNode::~PointCloudToFlatScanNode() {} + +} // namespace pointcloud_utils +} // namespace isaac_ros +} // namespace nvidia + +RCLCPP_COMPONENTS_REGISTER_NODE(nvidia::isaac_ros::pointcloud_utils::PointCloudToFlatScanNode) diff --git a/isaac_ros_pointcloud_utils/test/isaac_ros_pointcloud_to_flatscan_pol_test.py b/isaac_ros_pointcloud_utils/test/isaac_ros_pointcloud_to_flatscan_pol_test.py new file mode 100644 index 0000000..15a6b1c --- /dev/null +++ b/isaac_ros_pointcloud_utils/test/isaac_ros_pointcloud_to_flatscan_pol_test.py @@ -0,0 +1,111 @@ +# SPDX-FileCopyrightText: NVIDIA CORPORATION & AFFILIATES +# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# SPDX-License-Identifier: Apache-2.0 + +import os +import pathlib +import time + +from isaac_ros_pointcloud_interfaces.msg import FlatScan +from isaac_ros_test import IsaacROSBaseTest +from launch.actions import ExecuteProcess +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + +import pytest +import rclpy + + +@pytest.mark.rostest +def generate_test_description(): + """ + Isaac ROS Point Cloud to Flatscan Converter. + + This test publishes "Point Clouds" from a rosbag + and checks if a "flatscan" received at the output + of the flatenner is of the expected size. + """ + pointcloud_to_flatscan_node = ComposableNode( + package='isaac_ros_pointcloud_utils', + plugin='nvidia::isaac_ros::pointcloud_utils::PointCloudToFlatScanNode', + name='pointcloud_to_flatscan', + namespace=IsaacROSPointCloudToFlatScanPOLTest.generate_namespace() + ) + + rosbag_play = ExecuteProcess( + cmd=['ros2', 'bag', 'play', os.path.dirname(__file__) + + '/../data/rosbags/pointcloud', '-l', '-d', '4', + '--remap', + '/pointcloud:=' + + IsaacROSPointCloudToFlatScanPOLTest.generate_namespace() + + '/pointcloud'], + output='screen' + ) + + pointcloud_to_flatscan_container = ComposableNodeContainer( + package='rclcpp_components', + name='pointcloud_to_flatscan_container', + namespace='', + executable='component_container_mt', + composable_node_descriptions=[ + pointcloud_to_flatscan_node, + ], + output='screen' + ) + return IsaacROSPointCloudToFlatScanPOLTest.generate_test_description([ + rosbag_play, + pointcloud_to_flatscan_container + ]) + + +class IsaacROSPointCloudToFlatScanPOLTest(IsaacROSBaseTest): + """Test for Isaac ROS PointCloudToFlatScan Proof of Life.""" + + filepath = pathlib.Path(os.path.dirname(__file__)) + + def test_pointcloud_to_flatscan_pipeline(self) -> None: + """Expect the pipeline to produce FlatScan data from a PointCloud.""" + self.generate_namespace_lookup( + ['flatscan']) + + received_messages = {} + flatscan_sub, = self.create_logging_subscribers( + [('flatscan', FlatScan)], received_messages) + try: + # Wait at most TIMEOUT seconds for subscriber to respond + TIMEOUT = 20 + end_time = time.time() + TIMEOUT + + done = False + while time.time() < end_time: + rclpy.spin_once(self.node, timeout_sec=0.1) + # If we have received exactly one message on the output topic, break + if 'flatscan' in received_messages: + done = True + break + + self.assertTrue( + done, "Didn't receive output on flatscan topic!") + + flatscan_message = received_messages['flatscan'] + print(len(flatscan_message.ranges)) + self.assertTrue(len(flatscan_message.ranges) == 8892, + 'Size of range arrray is not as expected!') + + finally: + pass + self.assertTrue(self.node.destroy_subscription( + flatscan_sub)) diff --git a/resources/isaac_ros_map_localization_nodegraph.png b/resources/isaac_ros_map_localization_nodegraph.png new file mode 100644 index 0000000..cb09a16 --- /dev/null +++ b/resources/isaac_ros_map_localization_nodegraph.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8df2665ad232ec8ed1d819db3d6c98b6e2c50a1e8dc4d664288b7d611654711a +size 39445 diff --git a/resources/nav2_isaac_sim.gif b/resources/nav2_isaac_sim.gif new file mode 100644 index 0000000..c2a17b3 --- /dev/null +++ b/resources/nav2_isaac_sim.gif @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0f630a1c5c4d0292a7d59af6428cd7bec31563fde3788309c81e432ad795ab63 +size 30765386 diff --git a/resources/nav2_isaac_sim.png b/resources/nav2_isaac_sim.png new file mode 100644 index 0000000..e48edfb --- /dev/null +++ b/resources/nav2_isaac_sim.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9713480e1a892498415db3cd0fc36b5f8dc75d4803208208b5bd7bd946222bac +size 1854326 diff --git a/resources/occupancy_grid_localizer.gif b/resources/occupancy_grid_localizer.gif new file mode 100644 index 0000000..c82df47 --- /dev/null +++ b/resources/occupancy_grid_localizer.gif @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1fb9b26cd79634e469f56b1b2a526b82a30a2eaa2ff2e5359ec1541198bdbb3e +size 270151 diff --git a/resources/quickstart.png b/resources/quickstart.png new file mode 100644 index 0000000..c1061cd --- /dev/null +++ b/resources/quickstart.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1460a9ade7ec1604d238fc74c044626a658d8a23c69540afb12973a17bc37d76 +size 134994