image subscribe

Author: amilearning

elevation_mapping_cupy/config/core/core_param.yaml

@@ -1,7 +1,8 @@
 elevation_mapping:
   ros__parameters:
-    #### Basic parameters ########    
-    enable_normal_arrow_publishing: false
+    #### Basic parameters ########
+    voxel_filter_size: 0.05
+    enable_normal_arrow_publishing: true
     cell_n: 40000                                    # number of cells in the map.
     data_type: 'float32'                            # data type for the map.
     average_weight: 0.5
@@ -11,7 +12,7 @@ elevation_mapping:
     min_filter_iteration: 3                      # number of iterations for min filter.
     initialized_variance: 10.0                     # initial variance for each cell.    
     resolution: 0.1                                # resolution in m.  
-    map_length: 20.0                                  # map's size in m.
+    map_length: 20.0                                 # map's size in m.
     sensor_noise_factor: 0.05                       # point's noise is sensor_noise_factor*z^2 (z is distance from sensor).
     mahalanobis_thresh: 2.0                         # points outside this distance is outlier.
     outlier_variance: 0.01                          # if point is outlier, add this value to the cell.
@@ -65,7 +66,7 @@ elevation_mapping:
     enable_pointcloud_publishing: false
     enable_drift_corrected_TF_publishing: false
     enable_normal_color: true                      # If true, the map contains 'color' layer corresponding to normal. Add 'color' layer to the publishers setting if you want to visualize.
-    enable_normal: true
+    
 
     #### 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.
@@ -104,23 +105,29 @@ elevation_mapping:
     #    camera_info_topic_name: '/camera/depth/camera_info'
     #    channel_info_topic_name: '/camera/channel_info'
 
+    
     subscribers:
       pointcloud1:
-        # topic_name: '/ouster/points'   
-        topic_name: '/a200/sensors/camera_0/points'      
+        # topic_name: '/ouster/points'      
+        topic_name: '/a200/sensors/camera_0/points'    
         data_type: 'pointcloud'
-      # image1:
+      image1:
+        topic_name: '/feat_processing_node/semantic_seg_feat'        
+        info_name: '/feat_processing_node/a200/sensors/camera_0/color/camera_info_resized'
+        channel_name: '/feat_processing_node/feat_channel_info'
+        data_type: 'image'
+      # image2:
       #   topic_name: '/a200/sensors/camera_0/color/image'        
       #   info_name: '/a200/sensors/camera_0/color/camera_info'
       #   data_type: 'image'
         # channel_name: '/front_cam/channel_info'  
 
+        # channel_name: '/front_cam/channel_info'
+
       # image_topic2: '/camera/rgb/image_raw2'        
       # image_info2: '/camera/depth/camera_info2'
       # image_channel_info1: '/front_cam/channel_info'  
 
-      
-      
       # image: # for semantic images
       #   topic_name: '/front_cam/semantic_image'
       #   camera_info_topic_name: '/front_cam/camera/depth/camera_info_resized'
@@ -135,17 +142,17 @@ elevation_mapping:
 
     publishers:
       elevation_map_raw:        
-        layers: ['elevation', 'traversability', 'variance','normal_x', 'normal_y', 'normal_z']
+        layers: ['elevation', 'traversability', 'variance','rgb', 'normal_x', 'normal_y', 'normal_z', 'feat_0','feat_1','feat_2','feat_3','feat_4']
         basic_layers: ['elevation']
         fps: 5.0
-      # elevation_map_recordable:
-      #   layers: ['elevation', 'traversability']
-      #   basic_layers: ['elevation', 'traversability']
-      #   fps: 2.0
-      # elevation_map_filter:
-      #   layers: ['min_filter', 'smooth', 'inpaint', 'elevation']
-      #   basic_layers: ['min_filter']
-      #   fps: 3.0
+      elevation_map_recordable:
+        layers: ['elevation', 'traversability']
+        basic_layers: ['elevation', 'traversability']
+        fps: 2.0
+      elevation_map_filter:
+        layers: ['min_filter', 'smooth', 'inpaint', 'elevation']
+        basic_layers: ['min_filter']
+        fps: 3.0
 
 
   # plugun info

elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp

@@ -48,6 +48,7 @@
 #include <pcl/PCLPointCloud2.h>
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
+#include <pcl/filters/voxel_grid.h>
 
 // OpenCV
 #include <opencv2/core.hpp>
@@ -89,23 +90,28 @@ class ElevationMappingNode : public rclcpp::Node {
   using CameraChannelSyncPtr = std::shared_ptr<CameraChannelSync>;
 
   // Subscriber and Synchronizer for Pointcloud messages
-  using PointCloudSubscriber = message_filters::Subscriber<sensor_msgs::msg::PointCloud2>;
-  using PointCloudSubscriberPtr = std::shared_ptr<PointCloudSubscriber>;
-  using PointCloudPolicy = message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::PointCloud2, elevation_map_msgs::msg::ChannelInfo>;
-  using PointCloudSync = message_filters::Synchronizer<PointCloudPolicy>;
-  using PointCloudSyncPtr = std::shared_ptr<PointCloudSync>;
+  // using PointCloudSubscriber = message_filters::Subscriber<sensor_msgs::msg::PointCloud2>;
+  // using PointCloudSubscriberPtr = std::shared_ptr<PointCloudSubscriber>;
+  // using PointCloudPolicy = message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::PointCloud2, elevation_map_msgs::msg::ChannelInfo>;
+  // using PointCloudSync = message_filters::Synchronizer<PointCloudPolicy>;
+  // using PointCloudSyncPtr = std::shared_ptr<PointCloudSync>;
 
  private:
-  void readParameters();
-  void setupMapPublishers();
-  void pointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::string& key);    
-  void inputPointCloud(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::vector<std::string>& channels);
-//   void inputImage(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const std::vector<std::string>& channels);
+void readParameters();
+void setupMapPublishers();
+void pointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::string& key);    
+void inputPointCloud(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::vector<std::string>& channels);
+  
+void inputImage(const sensor_msgs::msg::Image::ConstSharedPtr& image_msg,
+                  const sensor_msgs::msg::CameraInfo::ConstSharedPtr& camera_info_msg,
+                  const std::vector<std::string>& channels);
+
+
 // void imageCallback(const sensor_msgs::msg::Image::SharedPtr image_msg, const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg, const std::string& key);
 // void imageChannelCallback(const sensor_msgs::msg::Image::SharedPtr image_msg, const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg, const elevation_map_msgs::msg::ChannelInfo::SharedPtr channel_info_msg);
-void imageCallback(const std::shared_ptr<const sensor_msgs::msg::Image>& image_msg, const std::shared_ptr<const sensor_msgs::msg::CameraInfo>& camera_info_msg, const std::string& key);
-void imageChannelCallback(const std::shared_ptr<const sensor_msgs::msg::Image>& image_msg, const std::shared_ptr<const sensor_msgs::msg::CameraInfo>& camera_info_msg, const std::shared_ptr<const elevation_map_msgs::msg::ChannelInfo>& channel_info_msg);
-
+void imageCallback(const sensor_msgs::msg::Image::SharedPtr cloud, const std::string& key);
+void imageChannelCallback(const elevation_map_msgs::msg::ChannelInfo::SharedPtr chennel_info, const std::string& key);
+void imageInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr image_info, const std::string& key);
 //   void pointCloudChannelCallback(const sensor_msgs::PointCloud2& cloud, const elevation_map_msgs::ChannelInfoConstPtr& channel_info_msg);
 //   // void multiLayerImageCallback(const elevation_map_msgs::MultiLayerImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg);
 void publishAsPointCloud(const grid_map::GridMap& map) const;
@@ -139,12 +145,18 @@ visualization_msgs::msg::Marker vectorToArrowMarker(const Eigen::Vector3d& start
   rclcpp::Node::SharedPtr node_;
   // image_transport::ImageTransport it_;
   std::vector<rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr> pointcloudSubs_;
-  std::vector<ImageSubscriberPtr> imageSubs_;
-  std::vector<CameraInfoSubscriberPtr> cameraInfoSubs_;
-  std::vector<ChannelInfoSubscriberPtr> channelInfoSubs_;
-  std::vector<CameraSyncPtr> cameraSyncs_;
-  std::vector<CameraChannelSyncPtr> cameraChannelSyncs_;
-  std::vector<PointCloudSyncPtr> pointCloudSyncs_;
+
+  std::vector<rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr> imageSubs_;
+  std::vector<rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr> cameraInfoSubs_;
+  std::vector<rclcpp::Subscription<elevation_map_msgs::msg::ChannelInfo>::SharedPtr> channelInfoSubs_;
+  
+
+  // std::vector<ImageSubscriberPtr> imageSubs_;
+  // std::vector<CameraInfoSubscriberPtr> cameraInfoSubs_;
+  // std::vector<ChannelInfoSubscriberPtr> channelInfoSubs_;
+  // std::vector<CameraSyncPtr> cameraSyncs_;
+  // std::vector<CameraChannelSyncPtr> cameraChannelSyncs_;
+  // std::vector<PointCloudSyncPtr> pointCloudSyncs_;
   std::vector<rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr> mapPubs_;
 
 
@@ -206,6 +218,7 @@ visualization_msgs::msg::Marker vectorToArrowMarker(const Eigen::Vector3d& start
 
     double positionAlpha_;
     double orientationAlpha_;
+    double voxel_filter_size_;
 
     double recordableFps_;
     std::atomic_bool enablePointCloudPublishing_;
@@ -215,6 +228,9 @@ visualization_msgs::msg::Marker vectorToArrowMarker(const Eigen::Vector3d& start
     double initializeTfGridSize_;
     bool alwaysClearWithInitializer_;
     std::atomic_int pointCloudProcessCounter_;
+
+    std::map<std::string, std::pair<sensor_msgs::msg::CameraInfo, bool>> imageInfoReady_;
+    std::map<std::string, std::pair<elevation_map_msgs::msg::ChannelInfo, bool>> imageChannelReady_;
 };
 
 }  // namespace elevation_mapping_cupy