elevation_mapping_cupy on Orin

Follow https://github.com/leggedrobotics/elevation_mapping_cupyhttps://github.com/leggedrobotics/elevation_mapping_cupy

mkdir -p ws_em/src && cd ws_em/src
git clone https://github.com/leggedrobotics/elevation_mapping_cupy.git

Install python dependencies

Install pip first

sudo apt install python3-pip

cd elevation_mapping_cupy
pip3 install -r requirements.txt
pip3 install cupy==11.4.0
pip3 install Cython

If some version related error occured, reinstall the corresponding packages of the correct versions using the format of pip3 install shapely==1.7.1

Be sure to install cupy 1.14.0. Otherwise some incompatibility issue may occur.

Install ROS dependencies

Install ROS first following noetic/Installation/Ubuntu - ROS Wiki

Setup the Tsinghua mirrors following ros | 镜像站使用帮助 | 清华大学开源软件镜像站 | Tsinghua Open Source Mirror

sudo apt install ros-noetic-pybind11-catkin
sudo apt install ros-noetic-grid-map-core ros-noetic-grid-map-msgs

It's better to sudo apt install ros-noetic-grid-map*, otherwise, an error will occur later about missing grid-map packages.

Other dependencies

sudo apt install libopencv-dev libeigen3-dev libgmp-dev libmpfr-dev libboost-all-dev
sudo apt install libopenblas-dev
sudo apt install gfortran
sudo apt install libcgal-dev

Install torch for arm achitecture

pip3 install torch-1.12.0a0+2c916ef.nv22.3-cp38-cp38-linux_aarch64.whl

Need to install the version matching the python version, python3.8 in this case.

Install opencv from source

following How to install OpenCV on Ubuntu 20.04 - VITUXOpenCV is a library of computer vision abbreviated as Open Source Computer Vision Library. Open means it is an open-source library with bindings for python, C++…https://vitux.com/opencv_ubuntu/

sudo apt install build-essential cmake git pkg-config libgtk-3-dev \
libavcodec-dev libavformat-dev libswscale-dev libv4l-dev \
libxvidcore-dev libx264-dev libjpeg-dev libpng-dev libtiff-dev \
gfortran openexr libatlas-base-dev python3-dev python3-numpy \
libtbb2 libtbb-dev libdc1394-22-dev libopenexr-dev \
libgstreamer-plugins-base1.0-dev libgstreamer1.0-dev

mkdir ~/opencv_build && cd ~/opencv_build
git clone https://github.com/opencv/opencv.git

git clone https://github.com/opencv/opencv_contrib.git

cd ~/opencv_build/opencv
mkdir -p build && cd build

cmake -D CMAKE_BUILD_TYPE=RELEASE \
-D CMAKE_INSTALL_PREFIX=/usr/local \
-D INSTALL_C_EXAMPLES=ON \
-D INSTALL_PYTHON_EXAMPLES=ON \
-D OPENCV_GENERATE_PKGCONFIG=ON \
-D OPENCV_EXTRA_MODULES_PATH=~/opencv_build/opencv_contrib/modules \
-D BUILD_EXAMPLES=ON ..

Compile, type nproc to check the number of cores.

make -j12

sudo make install

To verify c++ binding

pkg-config --modversion opencv4

To verify python binding

python3 -c "import cv2; print(cv2.__version__)"

 Create a soft link to the installed opencv2 folder in the /usr/include folder

sudo ln -s /usr/local/include/opencv4/opencv2 /usr/include/opencv2

 Add the soft link to path by adding the following line into ~/.bashrc or ~/.zshrc

export PATH=/usr/include:$PATH

Build

Make sure catkin tools are installed

sudo apt install python3-catkin-tools

Restart terminal or source ~/.bashrc, then

catkin build

Run

roslaunch elevation_mapping_cupy elevation_mapping_cupy.launch

For the following error,

terminate called after throwing an instance of 'pybind11::error_already_set'
  what():  ModuleNotFoundError: No module named 'cupy'

 find out where cupy is installed and add it to the launch file using the env tag

six_orin@ubuntu:~/ws_em$ pip3 uninstall cupy
Found existing installation: cupy 11.6.0
Uninstalling cupy-11.6.0:
  Would remove:
    /home/six_orin/.local/lib/python3.8/site-packages/cupy-11.6.0.dist-info/*
    /home/six_orin/.local/lib/python3.8/site-packages/cupy/*
    /home/six_orin/.local/lib/python3.8/site-packages/cupy_backends/*
    /home/six_orin/.local/lib/python3.8/site-packages/cupyx/*
Proceed (y/n)? 

<launch>
	<env name="PYTHONPATH" value="/home/six_orin/.local/lib/python3.8/site-packages:$(env PYTHONPATH)" />
	<!-- 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/parameters.yaml" />
	</node>
</launch>

 For the following error,

terminate called after throwing an instance of 'pybind11::error_already_set'
  what():  AttributeError: module 'typing' has no attribute '_ClassVar'

the suggested solution is to

pip3 uninstall dataclasses

To install the group customized version

Chainer is needed. The latest version 7.8.1 is not compatible with the cupy version, but can be ignored.

pip3 install chainer

Unsolved error

The plane segmentation process would die at startup, related to the following issue, no solutions provided.https://github.com/leggedrobotics/elevation_mapping_cupy/issues/37https://github.com/leggedrobotics/elevation_mapping_cupy/issues/37

sixorin@ubuntu:~$ export TURTLEBOT3_MODEL=waffle
sixorin@ubuntu:~$ roslaunch elevation_mapping_cupy turtlesim_segmentation_example.launch
... logging to /home/sixorin/.ros/log/2252d50e-7d0e-11ed-9d15-90e868844d4d/roslaunch-ubuntu-6526.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

xacro: in-order processing became default in ROS Melodic. You can drop the option.
started roslaunch server http://ubuntu:40295/

SUMMARY
========

PARAMETERS
 * /convex_plane_decomposition_ros/contour_extraction/marginSize: 1
 * /convex_plane_decomposition_ros/elevation_topic: /elevation_mappin...
 * /convex_plane_decomposition_ros/frequency: 20.0
 * /convex_plane_decomposition_ros/height_layer: elevation
 * /convex_plane_decomposition_ros/postprocessing/extracted_planes_height_offset: 0.0
 * /convex_plane_decomposition_ros/postprocessing/nonplanar_height_offset: 0.02
 * /convex_plane_decomposition_ros/postprocessing/nonplanar_horizontal_offset: 1
 * /convex_plane_decomposition_ros/postprocessing/smoothing_box_kernel_size: 0.1
 * /convex_plane_decomposition_ros/postprocessing/smoothing_dilation_size: 0.2
 * /convex_plane_decomposition_ros/postprocessing/smoothing_gauss_kernel_size: 0.05
 * /convex_plane_decomposition_ros/preprocessing/kernelSize: 3
 * /convex_plane_decomposition_ros/preprocessing/numberOfRepeats: 1
 * /convex_plane_decomposition_ros/preprocessing/resolution: 0.04
 * /convex_plane_decomposition_ros/publish_to_controller: True
 * /convex_plane_decomposition_ros/ransac_plane_refinement/cluster_epsilon: 0.041
 * /convex_plane_decomposition_ros/ransac_plane_refinement/epsilon: 0.025
 * /convex_plane_decomposition_ros/ransac_plane_refinement/min_points: 4
 * /convex_plane_decomposition_ros/ransac_plane_refinement/normal_threshold: 25.0
 * /convex_plane_decomposition_ros/ransac_plane_refinement/probability: 0.001
 * /convex_plane_decomposition_ros/sliding_window_plane_extractor/connectivity: 4
 * /convex_plane_decomposition_ros/sliding_window_plane_extractor/global_plane_fit_angle_error_threshold_degrees: 25.0
 * /convex_plane_decomposition_ros/sliding_window_plane_extractor/global_plane_fit_distance_error_threshold: 0.025
 * /convex_plane_decomposition_ros/sliding_window_plane_extractor/include_ransac_refinement: True
 * /convex_plane_decomposition_ros/sliding_window_plane_extractor/kernel_size: 3
 * /convex_plane_decomposition_ros/sliding_window_plane_extractor/local_plane_inclination_threshold_degrees: 35.0
 * /convex_plane_decomposition_ros/sliding_window_plane_extractor/min_number_points_per_label: 4
 * /convex_plane_decomposition_ros/sliding_window_plane_extractor/planarity_opening_filter: 0
 * /convex_plane_decomposition_ros/sliding_window_plane_extractor/plane_inclination_threshold_degrees: 30.0
 * /convex_plane_decomposition_ros/sliding_window_plane_extractor/plane_patch_error_threshold: 0.02
 * /convex_plane_decomposition_ros/submap/length: 3.0
 * /convex_plane_decomposition_ros/submap/width: 3.0
 * /convex_plane_decomposition_ros/target_frame_id: odom
 * /elevation_mapping/base_frame: base_footprint
 * /elevation_mapping/cleanup_cos_thresh: 0.1
 * /elevation_mapping/cleanup_step: 0.1
 * /elevation_mapping/corrected_map_frame: corrected_odom
 * /elevation_mapping/dilation_size: 3
 * /elevation_mapping/dilation_size_initialize: 5
 * /elevation_mapping/drift_compensation_alpha: 0.1
 * /elevation_mapping/drift_compensation_variance_inler: 0.05
 * /elevation_mapping/enable_drift_compensation: True
 * /elevation_mapping/enable_drift_corrected_TF_publishing: False
 * /elevation_mapping/enable_edge_sharpen: True
 * /elevation_mapping/enable_normal_color: False
 * /elevation_mapping/enable_overlap_clearance: True
 * /elevation_mapping/enable_pointcloud_publishing: False
 * /elevation_mapping/enable_visibility_cleanup: True
 * /elevation_mapping/initial_variance: 1000.0
 * /elevation_mapping/initialize_frame_id: ['base_footprint']
 * /elevation_mapping/initialize_method: linear
 * /elevation_mapping/initialize_tf_grid_size: 0.4
 * /elevation_mapping/initialize_tf_offset: [0.0]
 * /elevation_mapping/mahalanobis_thresh: 2.0
 * /elevation_mapping/map_acquire_fps: 5.0
 * /elevation_mapping/map_frame: odom
 * /elevation_mapping/map_length: 8
 * /elevation_mapping/max_drift: 0.1
 * /elevation_mapping/max_height_range: 1.0
 * /elevation_mapping/max_ray_length: 10.0
 * /elevation_mapping/max_unsafe_n: 10
 * /elevation_mapping/max_variance: 100.0
 * /elevation_mapping/min_height_drift_cnt: 100
 * /elevation_mapping/min_valid_distance: 0.5
 * /elevation_mapping/orientation_lowpass_alpha: 0.2
 * /elevation_mapping/orientation_noise_thresh: 0.01
 * /elevation_mapping/outlier_variance: 0.01
 * /elevation_mapping/overlap_clear_range_xy: 4.0
 * /elevation_mapping/overlap_clear_range_z: 2.0
 * /elevation_mapping/plugin_config_file: $(rospack find el...
 * /elevation_mapping/pointcloud_topics: ['/camera/depth/p...
 * /elevation_mapping/position_lowpass_alpha: 0.2
 * /elevation_mapping/position_noise_thresh: 0.01
 * /elevation_mapping/publish_statistics_fps: 1.0
 * /elevation_mapping/publishers/elevation_map_filter/basic_layers: ['min_filter']
 * /elevation_mapping/publishers/elevation_map_filter/fps: 3.0
 * /elevation_mapping/publishers/elevation_map_filter/layers: ['min_filter', 's...
 * /elevation_mapping/publishers/elevation_map_raw/basic_layers: ['elevation', 'tr...
 * /elevation_mapping/publishers/elevation_map_raw/fps: 5.0
 * /elevation_mapping/publishers/elevation_map_raw/layers: ['elevation', 'tr...
 * /elevation_mapping/publishers/elevation_map_recordable/basic_layers: ['elevation', 'tr...
 * /elevation_mapping/publishers/elevation_map_recordable/fps: 2.0
 * /elevation_mapping/publishers/elevation_map_recordable/layers: ['elevation', 'tr...
 * /elevation_mapping/ramped_height_range_a: 0.3
 * /elevation_mapping/ramped_height_range_b: 1.0
 * /elevation_mapping/ramped_height_range_c: 0.2
 * /elevation_mapping/resolution: 0.04
 * /elevation_mapping/safe_min_thresh: 0.4
 * /elevation_mapping/safe_thresh: 0.7
 * /elevation_mapping/sensor_noise_factor: 0.05
 * /elevation_mapping/time_interval: 0.1
 * /elevation_mapping/time_variance: 0.0001
 * /elevation_mapping/traversability_inlier: 0.9
 * /elevation_mapping/update_pose_fps: 10.0
 * /elevation_mapping/update_variance_fps: 5.0
 * /elevation_mapping/use_chainer: False
 * /elevation_mapping/use_initializer_at_start: True
 * /elevation_mapping/use_only_above_for_upper_bound: False
 * /elevation_mapping/wall_num_thresh: 20
 * /elevation_mapping/weight_file: $(rospack find el...
 * /gazebo/enable_ros_network: True
 * /robot_description: <?xml version="1....
 * /rosdistro: noetic
 * /rosversion: 1.15.15
 * /use_sim_time: True

NODES
  /
    convex_plane_decomposition_ros (convex_plane_decomposition_ros/convex_plane_decomposition_ros_node)
    elevation_mapping (elevation_mapping_cupy/elevation_mapping_node)
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)
    rviz (rviz/rviz)
    spawn_urdf (gazebo_ros/spawn_model)
    waffle_state_publisher (robot_state_publisher/robot_state_publisher)

auto-starting new master
process[master]: started with pid [6558]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 2252d50e-7d0e-11ed-9d15-90e868844d4d
process[rosout-1]: started with pid [6579]
started core service [/rosout]
process[gazebo-2]: started with pid [6586]
process[gazebo_gui-3]: started with pid [6591]
process[spawn_urdf-4]: started with pid [6596]
process[waffle_state_publisher-5]: started with pid [6597]
process[elevation_mapping-6]: started with pid [6598]
process[rviz-7]: started with pid [6599]
process[convex_plane_decomposition_ros-8]: started with pid [6601]
[convex_plane_decomposition_ros-8] process has died [pid 6601, exit code -11, cmd /home/sixorin/ws_em/devel/lib/convex_plane_decomposition_ros/convex_plane_decomposition_ros_node __name:=convex_plane_decomposition_ros __log:=/home/sixorin/.ros/log/2252d50e-7d0e-11ed-9d15-90e868844d4d/convex_plane_decomposition_ros-8.log].
log file: /home/sixorin/.ros/log/2252d50e-7d0e-11ed-9d15-90e868844d4d/convex_plane_decomposition_ros-8*.log
[ INFO] [1671173520.825106541]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1671173520.833573888]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[ INFO] [1671173521.416834797]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1671173521.427755939]: waitForService: Service [/gazebo_gui/set_physics_properties] has not been advertised, waiting...
[ INFO] [1671173524.329893900]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1671173526.007139707, 0.001000000]: Camera Plugin: Using the 'robotNamespace' param: '/'
[ INFO] [1671173526.053480057, 0.001000000]: Camera Plugin (ns = /)  <tf_prefix_>, set to ""
[ INFO] [1671173526.683113408, 0.001000000]: Laser Plugin: Using the 'robotNamespace' param: '/'
[ INFO] [1671173526.687467231, 0.001000000]: Starting Laser Plugin (ns = /)
[ INFO] [1671173526.713933373, 0.001000000]: Laser Plugin (ns = /)  <tf_prefix_>, set to ""
[ INFO] [1671173526.757111725, 0.001000000]: Physics dynamic reconfigure ready.
[ INFO] [1671173526.834593292, 0.001000000]: Starting plugin DiffDrive(ns = //)
[ INFO] [1671173526.835924512, 0.001000000]: DiffDrive(ns = //): <rosDebugLevel> = na
[ INFO] [1671173526.838661351, 0.001000000]: DiffDrive(ns = //): <tf_prefix> = 
[ INFO] [1671173526.843657167, 0.001000000]: DiffDrive(ns = //): Advertise joint_states
[ INFO] [1671173526.846505657, 0.001000000]: DiffDrive(ns = //): Try to subscribe to cmd_vel
[ INFO] [1671173526.851267293, 0.001000000]: DiffDrive(ns = //): Subscribe to cmd_vel
[ INFO] [1671173526.853743617, 0.001000000]: DiffDrive(ns = //): Advertise odom on odom 
[spawn_urdf-4] process has finished cleanly
log file: /home/sixorin/.ros/log/2252d50e-7d0e-11ed-9d15-90e868844d4d/spawn_urdf-4*.log
Start loading plugins...
Loaded plugins are  min_filter smooth_filter inpainting smooth_filter
[ INFO] [1671173530.263736159, 2.879000000]: [ElevationMappingCupy] finish initialization
[ INFO] [1671173530.467178403, 3.082000000]: Clearing map with initializer.
[ INFO] [1671173530.487253168, 3.102000000]: Initializing map with points using linear
Warning: TF_REPEATED_DATA ignoring data with redundant timestamp for frame base_footprint at time 3.233000 according to authority unknown_publisher
         at line 278 in /tmp/binarydeb/ros-noetic-tf2-0.7.6/src/buffer_core.cpp
[ WARN] [1671173530.696656461, 3.281000000]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame base_footprint at time 3.233000 according to authority unknown_publisher