Skip to content

Commit 970c643

Browse files
hemalshahNVhemalshahNV
authored
Merge pull request #8 from NVIDIA-ISAAC-ROS/hotfix-release-dp3.1-1
Update localizer parameters
2 parents 7de0fcc + 2bb088e commit 970c643

File tree

5 files changed

+36
-16
lines changed

5 files changed

+36
-16
lines changed

‎README.md‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -240,15 +240,17 @@ ros2 launch isaac_ros_occupancy_grid_localizer isaac_ros_occupancy_grid_localize
240240
| `map_yaml_path` | `std::string` | `""` | Absolute path to the map yaml file. From which we load the `resolution` and `occupied_thresh` |
241241
| `max_points` | `int` | `20000` | Maximum number of points in FlatScan Message that can be received used to pre-allocate GPU memory. |
242242
| `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. |
243+
| `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 |
244+
| `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 |
243245
| `max_beam_error` | `double` | `0.5` | The maximum beam error used when comparing range scans. |
244246
| `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. |
245247
| `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. |
248+
| `sample_distance` | `double` | `0.1` | Distance between sample points in meters. The smaller this number, the more sample poses will be considered. This leads to a higher accuracy and lower performance. |
246249
| `out_of_range_threshold` | `double` | `100.0` | Points range larger than this threshold will be marked as out of range and not used. |
247250
| `invalid_range_threshold` | `double` | `0.0` | Points range smaller than this threshold will be marked as invalid and not used. |
248251
| `min_scan_fov_degrees` | `double` | `270.0` | Minimal required scan fov to run the localizer. |
249252
| `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 |
250-
| `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 |
251-
| `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 |
253+
252254
253255
#### ROS Topics Subscribed
254256

‎isaac_ros_occupancy_grid_localizer/config/occupancy_grid_localizer_node.yaml‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -150,14 +150,16 @@ components:
150150
occupancy_2d_map_provider: occupancy_2d_map_provider/occupancy_2d_map_provider
151151
robot_radius: 0.25
152152
max_beam_error: 0.5
153+
max_output_error: 0.35
154+
min_output_error: 0.22
153155
num_beams_gpu: 512
154156
batch_size: 512
157+
sample_distance: 0.1
158+
patch_size: [512, 512]
155159
out_of_range_threshold: 100.0
156160
invalid_range_threshold: 0.0
157161
min_scan_fov_degrees: 270.0
158162
use_closest_beam: true
159-
min_output_error: 0.22
160-
max_output_error: 0.35
161163
---
162164
name: broadcaster
163165
components:

‎isaac_ros_occupancy_grid_localizer/include/isaac_ros_occupancy_grid_localizer/occupancy_grid_localizer_node.hpp‎

Lines changed: 13 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -52,9 +52,12 @@ class OccupancyGridLocalizerNode : public nitros::NitrosNode
5252

5353
OccupancyGridLocalizerNode & operator=(const OccupancyGridLocalizerNode &) = delete;
5454

55-
// The callback to be implemented by users for any required initialization
55+
// callback used for overiding gxf parameters
5656
void postLoadGraphCallback() override;
5757

58+
// callback used for overiding gxf parameters that is not supported by gxf functions
59+
void preLoadGraphCallback() override;
60+
5861
void GridSearchLocalizationCallback(
5962
const std::shared_ptr<std_srvs::srv::Empty::Request>,
6063
std::shared_ptr<std_srvs::srv::Empty::Response>);
@@ -97,13 +100,22 @@ class OccupancyGridLocalizerNode : public nitros::NitrosNode
97100
const double robot_radius_;
98101
// The maximum beam error used when comparing range scans.
99102
const double max_beam_error_;
103+
// The max output error from our best sample, if output error larger than this threshold, we
104+
// conclude localization failed
105+
const double max_output_error_;
106+
// The minimal output error used to normalize and compute confidence, if output error from best
107+
// sample smaller or equal to this, the confidence is 1
108+
const double min_output_error_;
100109
// The GPU accelerated scan-and-match function can only handle a certain number of beams per
101110
// range scan. The allowed values are {32, 64, 128, 256, 512}. If the number of beams in the
102111
// range scan does not match this number a subset of beams will be taken.
103112
const int num_beams_gpu_;
104113
// This is the number of scans to collect into a batch for the GPU kernel. Choose a value which
105114
// matches your GPU well.
106115
const int batch_size_;
116+
// Distance between sample points in meters. The smaller this number, the more sample poses
117+
// will be considered. This leads to a higher accuracy and lower performance.
118+
const double sample_distance_;
107119
// Points range larger than this threshold will be marked as out of range and not used.
108120
const double out_of_range_threshold_;
109121
// Points range smaller than this threshold will be marked as invalid and not used.
@@ -113,12 +125,6 @@ class OccupancyGridLocalizerNode : public nitros::NitrosNode
113125
// Whether or not pick the closest angle beam in angle bucket, if not pick the average within an
114126
// angular bucket
115127
const bool use_closest_beam_;
116-
// The minimal output error used to normalize and compute confidence, if output error from best
117-
// sample smaller or equal to this, the confidence is 1
118-
const double min_output_error_;
119-
// The max output error from our best sample, if output error larger than this threshold, we
120-
// conclude localization failed
121-
const double max_output_error_;
122128

123129
unsigned int map_png_height_;
124130
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

‎isaac_ros_occupancy_grid_localizer/src/occupancy_grid_localizer_node.cpp‎

Lines changed: 13 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -139,14 +139,15 @@ OccupancyGridLocalizerNode::OccupancyGridLocalizerNode(const rclcpp::NodeOptions
139139
use_gxf_map_convention_(declare_parameter<bool>("use_gxf_map_convention", false)),
140140
robot_radius_(declare_parameter<double>("robot_radius", 0.25)),
141141
max_beam_error_(declare_parameter<double>("max_beam_error", 0.5)),
142+
max_output_error_(declare_parameter<double>("max_output_error", 0.35)),
143+
min_output_error_(declare_parameter<double>("min_output_error", 0.22)),
142144
num_beams_gpu_(declare_parameter<int>("num_beams_gpu", 512)),
143145
batch_size_(declare_parameter<int>("batch_size", 512)),
146+
sample_distance_(declare_parameter<double>("sample_distance", 0.1)),
144147
out_of_range_threshold_(declare_parameter<double>("out_of_range_threshold", 100.0)),
145148
invalid_range_threshold_(declare_parameter<double>("invalid_range_threshold", 0.0)),
146149
min_scan_fov_degrees_(declare_parameter<double>("min_scan_fov_degrees", 270.0)),
147-
use_closest_beam_(declare_parameter<bool>("use_closest_beam", true)),
148-
min_output_error_(declare_parameter<double>("min_output_error", 0.22)),
149-
max_output_error_(declare_parameter<double>("max_output_error", 0.35))
150+
use_closest_beam_(declare_parameter<bool>("use_closest_beam", true))
150151
{
151152
RCLCPP_DEBUG(get_logger(), "[OccupancyGridLocalizerNode] Constructor");
152153

@@ -379,6 +380,15 @@ void OccupancyGridLocalizerNode::LocResultNitrosSubCallback(
379380
*pose_handle = ros_map_origin_transform * ros_pose_isaac_transform * (*pose_handle);
380381
}
381382

383+
void OccupancyGridLocalizerNode::preLoadGraphCallback()
384+
{
385+
RCLCPP_INFO(get_logger(), "[OccupancyGridLocalizerNode] preLoadGraphCallback().");
386+
// Setting gxf parameters not supported by gxf functions
387+
NitrosNode::preLoadGraphSetParameter(
388+
"binary_occupancy_map_loader", "nvidia::isaac::OccupancyGridMap", "sample_distance",
389+
std::to_string(sample_distance_));
390+
}
391+
382392
void OccupancyGridLocalizerNode::postLoadGraphCallback()
383393
{
384394
RCLCPP_DEBUG(get_logger(), "[OccupancyGridLocalizerNode] postLoadGraphCallback().");

‎isaac_ros_occupancy_grid_localizer/test/isaac_ros_occupancy_grid_localizer_pol_test.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -31,8 +31,8 @@
3131
import rclpy
3232
from std_srvs.srv import Empty
3333

34-
TEST_POS_TOLERANCE = 0.01
35-
TEST_QUAT_TOLERANCE = 0.001
34+
TEST_POS_TOLERANCE = 0.15
35+
TEST_QUAT_TOLERANCE = 0.013
3636
TEST_COV_TOLERANCE = 0.001
3737

3838

0 commit comments

Comments
 (0)