fixed python 3.8 dependencies in docker

docker/Dockerfile.x64

@@ -18,7 +18,8 @@ RUN apt-get update && apt-get install -y \
 # Install PyTorch
 COPY requirements.txt /tmp/requirements.txt
 RUN pip3 install -r /tmp/requirements.txt
-RUN pip3 install torch torchvision torchaudio --index-url https://download.pytorch.org/whl/cu118
+RUN pip install torch==1.13.1+cu117 torchvision==0.14.1+cu117 torchaudio==0.13.1 --index-url https://download.pytorch.org/whl/cu117
+
 
 # Install other Python packages
 RUN pip3 install cupy-cuda11x scikit-learn
@@ -43,4 +44,4 @@ RUN apt install -y ros-noetic-pybind11-catkin \
     ros-noetic-ros-numpy
 
 RUN pip3 install catkin-tools
-ENV TURTLEBOT3_MODEL=waffle
+ENV TURTLEBOT3_MODEL=waffle

elevation_mapping_cupy/config/setups/b1/b1_parameters.yaml

@@ -0,0 +1,89 @@
+#### Basic parameters ########
+resolution: 0.025                                # resolution in m.
+map_length: 3                                  # map's size in m.
+sensor_noise_factor: 0.1                       # point's noise is sensor_noise_factor*z^2 (z is distance from sensor).
+mahalanobis_thresh: 1.0                         # points outside this distance is outlier.
+outlier_variance: 0.05                          # if point is outlier, add this value to the cell.
+drift_compensation_variance_inler: 0.05         # cells under this value is used for drift compensation.
+max_drift: 0.1                                  # drift compensation happens only the drift is smaller than this value (for safety)
+drift_compensation_alpha: 0.1                   # drift compensation alpha for smoother update of drift compensation
+time_variance: 0.0001                           # add this value when update_variance is called.
+max_variance: 100.0                             # maximum variance for each cell.
+initial_variance: 1000.0                        # initial variance for each cell.
+traversability_inlier: 0.9                      # cells with higher traversability are used for drift compensation.
+dilation_size: 3                                # dilation filter size before traversability filter.
+wall_num_thresh: 20                             # if there are more points than this value, only higher points than the current height are used to make the wall more sharp.
+min_height_drift_cnt: 100                       # drift compensation only happens if the valid cells are more than this number.
+position_noise_thresh: 0.01                     # if the position change is bigger than this value, the drift compensation happens.
+orientation_noise_thresh: 0.01                  # if the orientation change is bigger than this value, the drift compensation happens.
+position_lowpass_alpha: 0.2                     # lowpass filter alpha used for detecting movements.
+orientation_lowpass_alpha: 0.2                  # lowpass filter alpha used for detecting movements.
+min_valid_distance: 0.06                         # points with shorter distance will be filtered out.
+max_height_range: 1.0                           # points higher than this value from sensor will be filtered out to disable ceiling.
+ramped_height_range_a: 0.3                      # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
+ramped_height_range_b: 1.0                      # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
+ramped_height_range_c: 0.2                      # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject.
+update_variance_fps: 5.0                        # fps for updating variance.
+update_pose_fps: 10.0                           # fps for updating pose and shift the center of map.
+time_interval: 0.1                              # Time layer is updated with this interval.
+map_acquire_fps: 5.0                            # Raw map is fetched from GPU memory in this fps.
+publish_statistics_fps: 1.0                     # Publish statistics topic in this fps.
+
+max_ray_length: 10.0                            # maximum length for ray tracing.
+cleanup_step: 0.3                               # subtitute this value from validity layer at visibiltiy cleanup.
+cleanup_cos_thresh: 0.3                         # subtitute this value from validity layer at visibiltiy cleanup.
+
+safe_thresh: 0.7                                # if traversability is smaller, it is counted as unsafe cell.
+safe_min_thresh: 0.4                            # polygon is unsafe if there exists lower traversability than this.
+max_unsafe_n: 10                                # if the number of cells under safe_thresh exceeds this value, polygon is unsafe.
+
+overlap_clear_range_xy: 2.0                     # xy range [m] for clearing overlapped area. this defines the valid area for overlap clearance. (used for multi floor setting)
+overlap_clear_range_z: 0.8                      # z range [m] for clearing overlapped area. cells outside this range will be cleared. (used for multi floor setting)
+
+map_frame: 'odom'                               # The map frame where the odometry source uses.
+base_frame: 'base'                              # The robot's base frame. This frame will be a center of the map.
+corrected_map_frame: 'odom'
+
+#### Feature toggles ########
+enable_edge_sharpen: true
+enable_visibility_cleanup: true
+enable_drift_compensation: false
+enable_overlap_clearance: true
+enable_pointcloud_publishing: false
+enable_drift_corrected_TF_publishing: false
+enable_normal_color: false                      # If true, the map contains 'color' layer corresponding to normal. Add 'color' layer to the publishers setting if you want to visualize.
+
+#### Traversability filter ########
+use_chainer: false                              # Use chainer as a backend of traversability filter or pytorch. If false, it uses pytorch. pytorch requires ~2GB more GPU memory compared to chainer but runs faster.
+weight_file: '$(rospack find elevation_mapping_cupy)/config/core/weights.dat'               # Weight file for traversability filter
+
+#### Upper bound ########
+use_only_above_for_upper_bound: false
+
+#### Plugins ########
+# plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/anymal_plugin_config.yaml'
+
+#### Publishers ########
+# topic_name:
+#   layers:               # Choose from 'elevation', 'variance', 'traversability', 'time', 'normal_x', 'normal_y', 'normal_z', 'color', plugin_layer_names
+#   basic_layers:         # basic_layers for valid cell computation (e.g. Rviz): Choose a subset of `layers`.
+#   fps:                  # Publish rate. Use smaller value than `map_acquire_fps`.
+
+# publishers:
+#   elevation_map_raw:
+#     layers: ['elevation', 'traversability', 'variance']
+#     basic_layers: ['elevation', 'traversability']
+#     fps: 5.0
+
+#   semantic_map_raw:
+#     layers: ['elevation', 'traversability']
+#     basic_layers: ['elevation', 'traversability']
+#     fps: 5.0
+
+#### Initializer ########
+initialize_method: 'linear'                                         # Choose one from 'nearest', 'linear', 'cubic'
+initialize_frame_id: ['FR_foot', 'FL_foot', 'RR_foot', 'RL_foot']   # One tf (like ['footprint'] ) initializes a square around it.
+initialize_tf_offset: [-0.031, -0.031,-0.031,-0.031]                          # z direction. Should be same number as initialize_frame_id.
+dilation_size_initialize: 15                                         # dilation size after the init.
+initialize_tf_grid_size: 0.4                                        # This is not used if number of tf is more than 3.
+use_initializer_at_start: true                                      # Use initializer when the node starts.

elevation_mapping_cupy/config/setups/b1/b1_sensor_parameter.yaml

@@ -0,0 +1,59 @@
+plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/setups/anymal/anymal_plugin_config.yaml'
+
+pointcloud_channel_fusions:
+  rgb: 'color'
+  default: 'average'
+
+image_channel_fusions:
+  rgb: 'color'
+  default: 'exponential'
+  feat_.*: 'exponential'
+  sem_.*: 'exponential'
+
+#### Publishers ########
+# topic_name:
+#   layers:               # Choose from 'elevation', 'variance', 'traversability', 'time', 'normal_x', 'normal_y', 'normal_z', 'color', plugin_layer_names
+#   basic_layers:         # basic_layers for valid cell computation (e.g. Rviz): Choose a subset of `layers`.
+#   fps:                  # Publish rate. Use smaller value than `map_acquire_fps`.
+
+publishers:
+  elevation_map_raw:
+    layers: ['elevation', 'traversability', 'variance', 'rgb', 'upper_bound']
+    basic_layers: ['elevation', 'traversability']
+    fps: 2.0
+
+  elevation_map_recordable:
+    layers: ['elevation', 'traversability', 'variance', 'rgb']
+    basic_layers: ['elevation', 'traversability']
+    fps: 5.0
+
+  filtered_elevation_map:
+    layers: ['inpaint', 'smooth', 'min_filter', 'max_filter', 'upper_bound']
+    basic_layers: ['inpaint']
+    fps: 1.0
+
+#### Subscribers ########
+subscribers:
+  # face_depth:
+  #   topic_name: /camera_face/depth/color/points
+  #   data_type: pointcloud
+
+  chin_depth:
+    topic_name: /camera_chin/depth/color/points
+    data_type: pointcloud
+
+  left_depth:
+    topic_name: /camera_left/depth/color/points
+    data_type: pointcloud
+
+  right_depth:
+    topic_name: /camera_right/depth/color/points
+    data_type: pointcloud
+
+  rear_depth:
+    topic_name: /camera_rear/depth/color/points_norobot
+    data_type: pointcloud
+
+  lidar_depth:
+    topic_name: /livox/lidar_norobot
+    data_type: pointcloud

elevation_mapping_cupy/launch/b1.launch

@@ -0,0 +1,7 @@
+<launch>
+    <!-- Elevation mapping node -->
+    <node pkg="elevation_mapping_cupy" type="elevation_mapping_node" name="elevation_mapping" output="screen">
+        <rosparam command="load" file="$(find elevation_mapping_cupy)/config/setups/b1/b1_parameters.yaml"/>
+        <rosparam command="load" file="$(find elevation_mapping_cupy)/config/setups/b1/b1_sensor_parameter.yaml"/>
+    </node>
+</launch>