Got running with cpp version. still needs work, but somewhat functional

Author: jwag

elevation_mapping_cupy/config/core/core_param.yaml

@@ -75,7 +75,7 @@ elevation_mapping_node:
     initialize_frame_id: ['base_link']   # 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: 2                                         # dilation size after the init.
-    initialize_tf_grid_size: 0.5                                        # This is not used if number of tf is more than 3.
+    initialize_tf_grid_size: 1.0                                        # This is not used if number of tf is more than 3.
     use_initializer_at_start: true                                      # Use initializer when the node starts.
 
     #### Default Plugins ########

elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp

@@ -68,7 +68,7 @@ namespace elevation_mapping_cupy {
 
 class ElevationMappingNode : public rclcpp::Node {
  public:
-  ElevationMappingNode();
+  ElevationMappingNode(const rclcpp::NodeOptions& options);
   using RowMatrixXd = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
   using ColMatrixXf = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic>;
 

elevation_mapping_cupy/src/elevation_mapping_node.cpp

@@ -12,9 +12,14 @@
 
 int main(int argc, char** argv) {
   rclcpp::init(argc, argv);
+
+  // Allow declaring parameters from the yaml files
+  rclcpp::NodeOptions options;
+  options.automatically_declare_parameters_from_overrides(true);
+  options.allow_undeclared_parameters(true);
 
   py::scoped_interpreter guard{};  // start the interpreter and keep it alive
-  auto mapNode = std::make_shared<elevation_mapping_cupy::ElevationMappingNode>();  // Correct instantiation  
+  auto mapNode = std::make_shared<elevation_mapping_cupy::ElevationMappingNode>(options);
   py::gil_scoped_release release;
 
 

elevation_mapping_cupy/src/elevation_mapping_ros.cpp

@@ -22,9 +22,8 @@
 namespace elevation_mapping_cupy {
 
 
-
-ElevationMappingNode::ElevationMappingNode()
-    : rclcpp::Node("elevation_mapping_node", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)),     
+ElevationMappingNode::ElevationMappingNode(const rclcpp::NodeOptions& options)
+    : Node("elevation_mapping_node", options),
       node_(std::shared_ptr<ElevationMappingNode>(this, [](auto *) {})),
       // it_(node_),
       lowpassPosition_(0, 0, 0),
@@ -35,7 +34,8 @@ ElevationMappingNode::ElevationMappingNode()
       orientationAlpha_(0.1),
       enablePointCloudPublishing_(false),
       isGridmapUpdated_(false){
-      RCLCPP_INFO(this->get_logger(), "Initializing ElevationMappingNode...");
+  
+  RCLCPP_INFO(this->get_logger(), "Initializing ElevationMappingNode...");
 
   tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);// ROS2构造TransformBroadcaster
   tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
@@ -93,6 +93,7 @@ ElevationMappingNode::ElevationMappingNode()
   RCLCPP_INFO(this->get_logger(), "enable_drift_corrected_TF_publishing: %s", enableDriftCorrectedTFPublishing_ ? "true" : "false");
   RCLCPP_INFO(this->get_logger(), "use_initializer_at_start: %s", useInitializerAtStart_ ? "true" : "false");
   RCLCPP_INFO(this->get_logger(), "always_clear_with_initializer: %s", alwaysClearWithInitializer_ ? "true" : "false");
+  RCLCPP_INFO(this->get_logger(), "voxel_filter_size: %f", voxel_filter_size_);
 
   enablePointCloudPublishing_ = enablePointCloudPublishing;
 

elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp

@@ -46,11 +46,6 @@ void ElevationMappingWrapper::initialize(const std::shared_ptr<rclcpp::Node>& no
   auto threading = py::module::import("threading");
   py::gil_scoped_acquire acquire;
 
-  auto sys = py::module::import("sys");
-  auto path = sys.attr("path");
-  std::string module_path = ament_index_cpp::get_package_share_directory("elevation_mapping_cupy");
-  module_path = module_path + "/script";
-  path.attr("insert")(0, module_path);
 
   auto elevation_mapping = py::module::import("elevation_mapping_cupy.elevation_mapping");
   auto parameter = py::module::import("elevation_mapping_cupy.parameter");
@@ -117,45 +112,45 @@ void ElevationMappingWrapper::setParameters() {
       }
 
   }
+  
+  py::dict sub_dict;
+  // rclcpp::Parameter subscribers;
+  std::vector<std::string> parameter_prefixes;
+  auto parameters = node_->list_parameters(parameter_prefixes, 2); // List all parameters with a maximum depth of 10
 
-      py::dict sub_dict;
-      // rclcpp::Parameter subscribers;
-      std::vector<std::string> parameter_prefixes;
-      auto parameters = node_->list_parameters(parameter_prefixes, 2); // List all parameters with a maximum depth of 10
-
-
-    std::map<std::string, rclcpp::Parameter> subscriber_params;  
-    if (!node_->get_parameters("subscribers", subscriber_params)) {
-      RCLCPP_FATAL(node_->get_logger(), "There aren't any subscribers to be configured, the elevation mapping cannot be configured. Exit");
-      rclcpp::shutdown();
-    }       
-    auto unique_sub_names = extract_unique_names(subscriber_params);
-    for (const auto& name : unique_sub_names) {      
-        const char* const name_c = name.c_str();    
-        if (!sub_dict.contains(name_c)) {
-              sub_dict[name_c] = py::dict();
-            }
-      std::string topic_name;
-      if(node_->get_parameter("subscribers." + name + ".topic_name", topic_name)){               
-          const char* topic_name_cstr = "topic_name";
-          sub_dict[name_c][topic_name_cstr] = static_cast<std::string>(topic_name);
-          std::string data_type;
-          if(node_->get_parameter("subscribers." + name + ".data_type", data_type)){
-            const char* data_type_cstr = "data_type";
-            sub_dict[name_c][data_type_cstr] = static_cast<std::string>(data_type);
-          }
-          std::string info_name;
-          if(node_->get_parameter("subscribers." + name + ".data_type", info_name)){
-            const char* info_name_cstr = "info_name";
-            sub_dict[name_c][info_name_cstr] = static_cast<std::string>(info_name);
-          }
-          std::string channel_name;
-          if(node_->get_parameter("subscribers." + name + ".data_type", channel_name)){
-            const char* channel_name_cstr = "channel_name";
-            sub_dict[name_c][channel_name_cstr] = static_cast<std::string>(channel_name);
+
+  std::map<std::string, rclcpp::Parameter> subscriber_params;  
+  if (!node_->get_parameters("subscribers", subscriber_params)) {
+    RCLCPP_FATAL(node_->get_logger(), "There aren't any subscribers to be configured, the elevation mapping cannot be configured. Exit");
+    rclcpp::shutdown();
+  }
+  auto unique_sub_names = extract_unique_names(subscriber_params);
+  for (const auto& name : unique_sub_names) {      
+      const char* const name_c = name.c_str();    
+      if (!sub_dict.contains(name_c)) {
+            sub_dict[name_c] = py::dict();
           }
-      }
+    std::string topic_name;
+    if(node_->get_parameter("subscribers." + name + ".topic_name", topic_name)){               
+        const char* topic_name_cstr = "topic_name";
+        sub_dict[name_c][topic_name_cstr] = static_cast<std::string>(topic_name);
+        std::string data_type;
+        if(node_->get_parameter("subscribers." + name + ".data_type", data_type)){
+          const char* data_type_cstr = "data_type";
+          sub_dict[name_c][data_type_cstr] = static_cast<std::string>(data_type);
+        }
+        std::string info_name;
+        if(node_->get_parameter("subscribers." + name + ".data_type", info_name)){
+          const char* info_name_cstr = "info_name";
+          sub_dict[name_c][info_name_cstr] = static_cast<std::string>(info_name);
+        }
+        std::string channel_name;
+        if(node_->get_parameter("subscribers." + name + ".data_type", channel_name)){
+          const char* channel_name_cstr = "channel_name";
+          sub_dict[name_c][channel_name_cstr] = static_cast<std::string>(channel_name);
+        }
     }
+  }
 
 
 
@@ -199,14 +194,17 @@ void ElevationMappingWrapper::setParameters() {
   }
 
 
-      param_.attr("update")();
-      resolution_ = py::cast<float>(param_.attr("get_value")("resolution"));
-      map_length_ = py::cast<float>(param_.attr("get_value")("true_map_length"));
-      map_n_ = py::cast<int>(param_.attr("get_value")("true_cell_n"));
-
-      
-      
-      enable_normal_color_ = node_->get_parameter("enable_normal_color").as_bool();
+    // Update the cell_n parameters based on the map length and resolution
+  RCLCPP_INFO(node_->get_logger(), "Updating cell_n parameters based on map length and resolution");
+  param_.attr("update")();
+  resolution_ = py::cast<float>(param_.attr("get_value")("resolution"));
+  map_length_ = py::cast<float>(param_.attr("get_value")("true_map_length"));
+  map_n_ = py::cast<int>(param_.attr("get_value")("true_cell_n"));
+  RCLCPP_INFO(node_->get_logger(), "cell_n: %d", map_n_);
+  RCLCPP_INFO(node_->get_logger(), "resolution: %f", resolution_);
+  RCLCPP_INFO(node_->get_logger(), "true_map_length: %f", map_length_);
+  
+  enable_normal_color_ = node_->get_parameter("enable_normal_color").as_bool();
 
 }