implemented cpp part for universal semantic mapping. I did not format the code yet as it would be hard to recognize the canges

Author: gerni17

.gitignore

@@ -1,3 +1,4 @@
 *.pyc
 *.bkp
 *.orig
+.idea

elevation_mapping_cupy/config/parameters.yaml

@@ -40,9 +40,9 @@ max_unsafe_n: 10                                # if the number of cells under s
 overlap_clear_range_xy: 4.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: 2.0                      # 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: 'corrected_odom'
+map_frame: 'enu'                               # The map frame where the odometry source uses.
+base_frame: 'base_footprint'                              # The robot's base frame. This frame will be a center of the map.
+corrected_map_frame: 'enu'
 
 #### Feature toggles ########
 enable_edge_sharpen: true
@@ -64,13 +64,7 @@ use_only_above_for_upper_bound: false
 plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/plugin_config.yaml'
 
 #### Subscribers ########
-pointcloud_topics: ['/depth_camera_front/depth/color/points',
-                    '/depth_camera_rear/depth/color/points',
-                    '/depth_camera_left/depth/color/points',
-                    '/depth_camera_right/depth/color/points',
-                    '/robot_self_filter/bpearl_front/point_cloud',
-                    '/robot_self_filter/bpearl_rear/point_cloud'
-                    ]
+
 
 #### Publishers ########
 # topic_name:
@@ -93,7 +87,7 @@ publishers:
 
 #### Initializer ########
 initialize_method: 'linear'                                         # Choose one from 'nearest', 'linear', 'cubic'
-initialize_frame_id: ['RF_FOOT', 'LF_FOOT', 'RH_FOOT', 'LH_FOOT']   # One tf (like ['footprint'] ) initializes a square around it.
+initialize_frame_id: ['base_footprint']   # One tf (like ['footprint'] ) initializes a square around it.
 initialize_tf_offset: [0.0, 0.0, 0.0, 0.0]                          # z direction. Should be same number as initialize_frame_id.
 dilation_size_initialize: 5                                         # dilation size after the init.
 initialize_tf_grid_size: 0.4                                        # This is not used if number of tf is more than 3.

elevation_mapping_cupy/config/sensor_parameter.yaml

@@ -0,0 +1,9 @@
+subscribers:
+#  sensor_name:
+#    channels: ['feat_0','feat_1']
+#    fusion: ['average','average']
+#    topic_name: '/elevation_mapping/pointcloud_semantic'
+  front_cam:
+    channels: ['feat_0','feat_1']
+    fusion: ['average','average']
+    topic_name: '/elvation_mapping/pointcloud_semantic'

elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp

@@ -48,6 +48,7 @@ namespace elevation_mapping_cupy {
 class ElevationMappingNode {
  public:
   ElevationMappingNode(ros::NodeHandle& nh);
+  using RowMatrixXd = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
 
  private:
   void readParameters();

elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp

@@ -13,6 +13,7 @@
 
 // Pybind
 #include <pybind11_catkin/pybind11/embed.h>  // everything needed for embedding
+#include <pybind11/stl.h>
 
 // ROS
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
@@ -42,11 +43,11 @@ class ElevationMappingWrapper {
 
   ElevationMappingWrapper();
 
-  void initialize(ros::NodeHandle& nh);
+  void initialize(ros::NodeHandle& nh, std::vector<std::string> additional_layers, std::vector<std::string> fusion_algorithms);
 
-  void input(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointCloud, const RowMatrixXd& R, const Eigen::VectorXd& t,
+  void input(const RowMatrixXd& points, const std::vector<std::string>& channels, const RowMatrixXd& R, const Eigen::VectorXd& t,
              const double positionNoise, const double orientationNoise);
-  void move_to(const Eigen::VectorXd& p);
+  void move_to(const Eigen::VectorXd& p, const RowMatrixXd& R);
   void clear();
   void update_variance();
   void update_time();
@@ -57,11 +58,10 @@ class ElevationMappingWrapper {
                                   std::vector<Eigen::Vector2d>& untraversable_polygon);
   double get_additive_mean_error();
   void initializeWithPoints(std::vector<Eigen::Vector3d>& points, std::string method);
-  void pointCloudToMatrix(const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointCloud, RowMatrixXd& points);
   void addNormalColorLayer(grid_map::GridMap& map);
 
  private:
-  void setParameters(ros::NodeHandle& nh);
+  void setParameters(ros::NodeHandle& nh, std::vector<std::string> additional_layers, std::vector<std::string> fusion_algorithms);
   py::object map_;
   py::object param_;
   double resolution_;

elevation_mapping_cupy/launch/semantic_elevation_lonomy_single.launch

@@ -0,0 +1,23 @@
+<launch>
+    <arg name="use_sim_time" default="true" />
+    <arg name="model" default="$(find rowesys_description)/urdf/rowesys.urdf.xacro"/>
+
+    <param name="/use_sim_time" type="bool" value="$(arg use_sim_time)" />
+    <param name="robot_description" command="$(find xacro)/xacro '--inorder' $(arg model)" />
+
+<!--    <node pkg="elevation_mapping_cupy" type="pointcloud.py" name="pointcloud" output="screen" >-->
+<!--        <param name="/use_sim_time" type="boolean" value="$(arg use_sim_time)" />-->
+<!--        <param name="cam_name" type="string" value="semantic_parameters" />-->
+<!--        <rosparam command="load" file="$(find elevation_mapping_cupy)/config/semantic_parameters.yaml" />-->
+<!--    </node>-->
+
+    <!-- Elevation mapping node -->
+    <node pkg="elevation_mapping_cupy" type="elevation_mapping_node" name="elevation_mapping" output="screen"  >
+        <param name="/use_sim_time" type="bool" value="$(arg use_sim_time)" />
+        <rosparam command="load" file="$(find elevation_mapping_cupy)/config/parameters.yaml" />
+        <rosparam command="load" file="$(find elevation_mapping_cupy)/config/sensor_parameter.yaml" />
+    </node>
+
+     <node name="rviz" pkg="rviz" type="rviz" args="-d $(find elevation_mapping_cupy)/rviz/lonomy_single.rviz"/>
+</launch>
+<!--        launch-prefix="gdb -ex run - -args"-->