Skip to content

Commit 4eb8103

Browse files
tutunarsltutunarsl
committed
Merge release
1 parent 3c52e60 commit 4eb8103

File tree

7 files changed

+396
-21
lines changed

7 files changed

+396
-21
lines changed

‎elevation_mapping_cupy/config/core/core_param.yaml‎

Lines changed: 16 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,14 @@
11
/elevation_mapping_node:
22
ros__parameters:
33
#### Basic parameters ########
4-
drift_compensation_variance_inlier: 0.1
4+
voxel_filter_size: 0.1
5+
average_weight: 0.5
6+
drift_compensation_variance_inlier: 0.05
57
checker_layer: 'elevation' # layer name for checking the validity of the cell.
68
initialized_variance: 10.0 # initial variance for each cell.
7-
resolution: 0.1 # resolution in m.
8-
map_length: 20.0 # map's size in m.
9-
sensor_noise_factor: 0.05 # point noise ~ sensor_noise_factor * range^2 in the sensor frame (range^2 = x^2+y^2+z^2).
9+
resolution: 0.05 # resolution in m.
10+
map_length: 8.0 # map's size in m.
11+
sensor_noise_factor: 0.05 # point's noise is sensor_noise_factor*z^2 (z is distance from sensor).
1012
mahalanobis_thresh: 2.0 # points outside this distance is outlier.
1113
outlier_variance: 0.01 # if point is outlier, add this value to the cell.
1214
max_drift: 0.1 # drift compensation happens only the drift is smaller than this value (for safety)
@@ -22,8 +24,8 @@
2224
orientation_noise_thresh: 0.01 # if the orientation change is bigger than this value, the drift compensation happens.
2325
position_lowpass_alpha: 0.2 # lowpass filter alpha used for detecting movements.
2426
orientation_lowpass_alpha: 0.2 # lowpass filter alpha used for detecting movements.
25-
min_valid_distance: 0.1 # points with shorter distance will be filtered out.
26-
max_height_range: 10.5 # points higher than this value from sensor will be filtered out to disable ceiling.
27+
min_valid_distance: 0.5 # points with shorter distance will be filtered out.
28+
max_height_range: 2.5 # points higher than this value from sensor will be filtered out to disable ceiling.
2729
ramped_height_range_a: 0.3 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
2830
ramped_height_range_b: 1.0 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
2931
ramped_height_range_c: 0.2 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
@@ -52,9 +54,9 @@
5254
overlap_clear_range_z: 2.0 # z range [m] for clearing overlapped area. cells outside this range will be cleared. (used for multi floor setting)
5355

5456
# Default frame configuration (can be overridden in robot-specific configs)
55-
map_frame: 'odom' # The map frame where the odometry source uses.
56-
base_frame: 'base_link' # The robot's base frame. This frame will be a center of the map.
57-
corrected_map_frame: 'odom'
57+
map_frame: 'odom_zedx' # The map frame where the odometry source uses.
58+
base_frame: 'zed_camera_link' # The robot's base frame. This frame will be a center of the map.
59+
corrected_map_frame: 'odom_zedx'
5860

5961
#### Feature toggles ########
6062
enable_edge_sharpen: true
@@ -71,8 +73,13 @@
7173

7274
#### Initializer ########
7375
initialize_method: 'linear' # Choose one from 'nearest', 'linear', 'cubic'
76+
<<<<<<< HEAD
7477
initialize_frame_id: ['base_link'] # One tf (like ['footprint'] ) initializes a square around it.
7578
initialize_tf_offset: [0.0] # Optional z-offset(s) for initializer (same length as initialize_frame_id).
79+
=======
80+
initialize_frame_id: ['zed_camera_link'] # One tf (like ['footprint'] ) initializes a square around it.
81+
initialize_tf_offset: [0.0, 0.0, 0.0, 0.0] # z direction. Should be same number as initialize_frame_id.
82+
>>>>>>> ptz
7683
dilation_size_initialize: 2 # dilation size after the init.
7784
initialize_tf_grid_size: 1.0 # This is not used if number of tf is more than 3.
7885
use_initializer_at_start: true # Use initializer when the node starts.

‎elevation_mapping_cupy/config/core/example_setup.yaml‎

Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
elevation_mapping:
2+
ros__parameters:
3+
#### Plugins ########
4+
plugin_config_file: '$(find_package_share elevation_mapping_cupy)/config/core/plugin_config.yaml'
5+
6+
#### Channel Fusions ########
7+
pointcloud_channel_fusions:
8+
rgb: 'color' # 'color' fusion is used for the 'rgb' channel
9+
default: 'average' # 'average' fusion is used for channels not listed here
10+
11+
image_channel_fusions:
12+
rgb: 'color' # 'color' fusion is used for the 'rgb' channel
13+
default: 'exponential' # 'exponential' fusion is used for channels not listed here
14+
feat_.*: 'exponential' # 'exponential' fusion is also used for any channel starting with 'feat_' Regular expressions can be used for channel names
15+
16+
#### Subscribers ########
17+
# pointcloud_sensor_name:
18+
# topic_name: '/sensor/pointcloud_semantic'
19+
# data_type: pointcloud # pointcloud or image
20+
#
21+
# image_sensor_name:
22+
# topic_name: '/camera/image_semantic'
23+
# data_type: image # pointcloud or image
24+
# camera_info_topic_name: '/camera/depth/camera_info'
25+
# channel_info_topic_name: '/camera/channel_info'
26+
27+
28+
subscribers:
29+
front_cam:
30+
topic_name: '/zed/zed_node/point_cloud/cloud_registered'
31+
data_type: pointcloud
32+
color_cam: # for color camera
33+
topic_name: '/camera/rgb/image_raw'
34+
camera_info_topic_name: '/camera/depth/camera_info'
35+
data_type: image
36+
semantic_cam: # for semantic images
37+
topic_name: '/front_cam/semantic_image'
38+
camera_info_topic_name: '/front_cam/camera/depth/camera_info_resized'
39+
channel_info_topic_name: '/front_cam/channel_info'
40+
data_type: image
41+
42+
#### Publishers ########
43+
# topic_name:
44+
# layers: # Choose from 'elevation', 'variance', 'traversability', 'time', 'normal_x', 'normal_y', 'normal_z', 'color', plugin_layer_names
45+
# basic_layers: # basic_layers for valid cell computation (e.g. Rviz): Choose a subset of `layers`.
46+
# fps: # Publish rate. Use smaller value than `map_acquire_fps`.
47+
48+
publishers:
49+
elevation_map_raw:
50+
layers: ['elevation', 'traversability', 'variance','rgb']
51+
basic_layers: ['elevation']
52+
fps: 5.0
53+
elevation_map_recordable:
54+
layers: ['elevation', 'traversability']
55+
basic_layers: ['elevation', 'traversability']
56+
fps: 2.0
57+
elevation_map_filter:
58+
layers: ['min_filter', 'smooth', 'inpaint', 'elevation']
59+
basic_layers: ['min_filter']
60+
fps: 3.0

‎elevation_mapping_cupy/config/setups/menzi/base.yaml‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -2,15 +2,15 @@
22
ros__parameters:
33
subscribers:
44
front_cam:
5-
topic_name: '/ouster_points_self_filtered'
5+
topic_name: '/zed/zed_node/point_cloud/cloud_registered'
66
data_type: pointcloud
77

88
publishers:
99
elevation_map_raw:
1010
layers: ['elevation', 'traversability', 'variance']
1111
basic_layers: ['elevation']
12-
fps: 5.0
12+
fps: 10.0
1313
elevation_map_filter:
14-
layers: ['min_filter', 'smooth', 'inpaint', 'elevation']
15-
basic_layers: ['min_filter']
16-
fps: 3.0
14+
layers: ['inpaint', 'smooth', 'min_filter', 'upper_bound']
15+
basic_layers: ['inpaint']
16+
fps: 5.0

‎elevation_mapping_cupy/elevation_mapping_cupy/parameter.py‎

Lines changed: 39 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -284,10 +284,44 @@ def update(self):
284284

285285
if __name__ == "__main__":
286286
param = Parameter()
287-
print(param)
288-
print(param.resolution)
287+
288+
print("\n" + "=" * 80)
289+
print("[INIT] Created Parameter() instance")
290+
print("=" * 80)
291+
print(f"[INIT] Full dataclass dump:\n{param!r}")
292+
293+
print("\n" + "-" * 80)
294+
print("[BEFORE UPDATE] Parameter 'resolution'")
295+
print("-" * 80)
296+
print(f"[BEFORE UPDATE] resolution value: {param.resolution}")
297+
print(f"[BEFORE UPDATE] resolution type : {type(param.resolution).__name__}")
298+
299+
print("\n" + "-" * 80)
300+
print("[ACTION] Updating 'resolution' via set_value('resolution', 0.1)")
301+
print("-" * 80)
289302
param.set_value("resolution", 0.1)
290-
print(param.resolution)
291303

292-
print("names ", param.get_names())
293-
print("types ", param.get_types())
304+
print("\n" + "-" * 80)
305+
print("[AFTER UPDATE] Parameter 'resolution'")
306+
print("-" * 80)
307+
print(f"[AFTER UPDATE] resolution value: {param.resolution}")
308+
print(f"[AFTER UPDATE] resolution type : {type(param.resolution).__name__}")
309+
310+
print("\n" + "=" * 80)
311+
print("[INTROSPECTION] Parameter metadata")
312+
print("=" * 80)
313+
names = param.get_names()
314+
types = param.get_types()
315+
316+
print(f"[INTROSPECTION] Total number of parameters: {len(names)}")
317+
print("[INTROSPECTION] Parameter names:")
318+
for i, name in enumerate(names, start=1):
319+
print(f" {i:02d}. {name}")
320+
321+
print("\n[INTROSPECTION] Parameter declared types:")
322+
for i, (name, ptype) in enumerate(zip(names, types), start=1):
323+
print(f" {i:02d}. {name}: {ptype}")
324+
325+
print("\n" + "=" * 80)
326+
print("[DONE] Verbose printout complete")
327+
print("=" * 80)

‎elevation_mapping_cupy/launch/dont_elevation_mapping_cupy.launch.py‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,33 @@
1+
from launch import LaunchDescription
2+
from launch_ros.actions import Node
3+
from launch.substitutions import PathJoinSubstitution
4+
from ament_index_python.packages import get_package_share_directory
5+
import launch_ros.actions
6+
7+
def generate_launch_description():
8+
elevation_mapping_cupy_dir = get_package_share_directory('elevation_mapping_cupy')
9+
10+
return LaunchDescription([
11+
launch_ros.actions.SetParameter(name='use_sim_time', value=False),
12+
Node(
13+
package='elevation_mapping_cupy',
14+
executable='elevation_mapping_node.py',
15+
name='elevation_mapping',
16+
parameters=[
17+
{'use_sim_time': True},
18+
PathJoinSubstitution([
19+
elevation_mapping_cupy_dir,
20+
'config',
21+
'core',
22+
'core_param.yaml'
23+
]),
24+
PathJoinSubstitution([
25+
elevation_mapping_cupy_dir,
26+
'config',
27+
'core',
28+
'example_setup.yaml'
29+
])
30+
],
31+
output='screen'
32+
)
33+
])

‎elevation_mapping_cupy/launch/elevation_mapping.launch.py‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,6 @@ def generate_launch_description():
1818
# Declare launch arguments
1919
robot_param_arg = DeclareLaunchArgument(
2020
'robot_config',
21-
# default_value='turtle_bot/turle_bot_simple.yaml',
2221
default_value='menzi/base.yaml',
2322
description='Name of the robot-specific config file within '
2423
'config/setups/'
@@ -30,6 +29,7 @@ def generate_launch_description():
3029
description='Whether to launch RViz'
3130
)
3231

32+
# elevation_mapping_cupy/elevation_mapping_cupy/rviz/ros2.rviz
3333
rviz_config_arg = DeclareLaunchArgument(
3434
'rviz_config',
3535
default_value='',
@@ -38,7 +38,7 @@ def generate_launch_description():
3838

3939
use_sim_time_arg = DeclareLaunchArgument(
4040
'use_sim_time',
41-
default_value='false',
41+
default_value='true',
4242
description='Use simulation clock if true'
4343
)
4444

@@ -61,6 +61,8 @@ def generate_launch_description():
6161
executable='elevation_mapping_node.py',
6262
name='elevation_mapping_node',
6363
output='screen',
64+
emulate_tty=True,
65+
additional_env={'PYTHONUNBUFFERED': '1'},
6466
parameters=[
6567
core_param_path,
6668
robot_param_path,

0 commit comments

Comments
 (0)