temporary progress

tier4 · Apr 4, 2024 · d839329 · d839329
1 parent 39d050c
commit d839329
Show file tree

Hide file tree

Showing 6 changed files with 78 additions and 127 deletions.
diff --git a/nebula_messages/nebula_msgs/CMakeLists.txt b/nebula_messages/nebula_msgs/CMakeLists.txt
@@ -21,4 +21,4 @@ rosidl_generate_interfaces(${PROJECT_NAME}
         std_msgs
         )
 
-ament_package()
+ament_package()
diff --git a/nebula_messages/nebula_msgs/msg/NebulaPacket.msg b/nebula_messages/nebula_msgs/msg/NebulaPacket.msg
@@ -1,2 +1,2 @@
 builtin_interfaces/Time stamp
-uint8[] data
+uint8[] data
diff --git a/nebula_messages/nebula_msgs/package.xml b/nebula_messages/nebula_msgs/package.xml
@@ -22,4 +22,4 @@
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>
diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp
@@ -124,10 +124,14 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase
   /// @return Resulting status
   Status InitializeHwInterface(
     const drivers::SensorConfigurationBase & sensor_configuration) override;
-  /// @brief Callback for receiving PandarScan
+  /// @brief Callback for receiving a raw UDP packet
   /// @param scan_buffer Received PandarScan
   void ReceiveCloudPacketCallback(const std::vector<uint8_t> & scan_buffer);
 
+  /// @brief Decodes a nebula packet and, if it completes the scan, publishes the pointcloud.
+  /// @param packet_msg The received packet message
+  void ProcessCloudPacket(std::unique_ptr<nebula_msgs::msg::NebulaPacket> packet_msg);
+
   /// @brief rclcpp parameter callback
   /// @param parameters Received parameters
   /// @return SetParametersResult

diff --git a/nebula_ros/launch/hesai_launch_all_hw.xml b/nebula_ros/launch/hesai_launch_all_hw.xml
@@ -33,32 +33,35 @@
     <let name="debug_level" value="debug" if="$(eval $(var debug_logging))"/>
     <let name="debug_level" value="info" unless="$(eval $(var debug_logging))"/>
 
-    <node pkg="nebula_ros" exec="hesai_ros_wrapper_node"
-           name="nebula_hesai" output="screen" ros_args="--log-level $(var debug_level)">
-        <param name="sensor_model" value="$(var sensor_model)"/>
-        <param name="return_mode" value="$(var return_mode)"/>
-        <param name="frame_id" value="$(var frame_id)"/>
-        <param name="scan_phase" value="$(var scan_phase)"/>
-        <param name="sensor_ip" value="$(var sensor_ip)"/>
-        <param name="calibration_file" value="$(var calibration_file)"/>
-        <param name="correction_file" value="$(var correction_file)"/>
-        <param name="launch_hw" value="$(var launch_hw)"/>
-
-        <param name="host_ip" value="$(var host_ip)"/>
-        <param name="data_port" value="$(var data_port)"/>
-        <param name="gnss_port" value="$(var gnss_port)"/>
-        <param name="packet_mtu_size" value="$(var packet_mtu_size)"/>
-        <param name="rotation_speed" value="$(var rotation_speed)"/>
-        <param name="cloud_min_angle" value="$(var cloud_min_angle)"/>
-        <param name="cloud_max_angle" value="$(var cloud_max_angle)"/>
-        <param name="dual_return_distance_threshold" value="$(var dual_return_distance_threshold)"/>
-        <param name="setup_sensor" value="$(var setup_sensor)"/>
-        <param name="retry_hw" value="$(var retry_hw)"/>
-        <param name="ptp_profile" value="$(var ptp_profile)"/>
-        <param name="ptp_domain" value="$(var ptp_domain)"/>
-        <param name="ptp_transport_type" value="$(var ptp_transport_type)"/>
-        <param name="ptp_switch_type" value="$(var ptp_switch_type)"/>
-
-        <param name="diag_span" value="$(var diag_span)"/>
-    </node>
+    <node_container pkg="rclcpp_components" exec="component_container_mt" name="nebula_container" 
+        output="screen" ros_args="--log-level $(var debug_level)" namespace="">
+        <composable_node pkg="nebula_ros" plugin="HesaiRosWrapper"
+               name="nebula_hesai">
+            <param name="sensor_model" value="$(var sensor_model)"/>
+            <param name="return_mode" value="$(var return_mode)"/>
+            <param name="frame_id" value="$(var frame_id)"/>
+            <param name="scan_phase" value="$(var scan_phase)"/>
+            <param name="sensor_ip" value="$(var sensor_ip)"/>
+            <param name="calibration_file" value="$(var calibration_file)"/>
+            <param name="correction_file" value="$(var correction_file)"/>
+            <param name="launch_hw" value="$(var launch_hw)"/>
+
+            <param name="host_ip" value="$(var host_ip)"/>
+            <param name="data_port" value="$(var data_port)"/>
+            <param name="gnss_port" value="$(var gnss_port)"/>
+            <param name="packet_mtu_size" value="$(var packet_mtu_size)"/>
+            <param name="rotation_speed" value="$(var rotation_speed)"/>
+            <param name="cloud_min_angle" value="$(var cloud_min_angle)"/>
+            <param name="cloud_max_angle" value="$(var cloud_max_angle)"/>
+            <param name="dual_return_distance_threshold" value="$(var dual_return_distance_threshold)"/>
+            <param name="setup_sensor" value="$(var setup_sensor)"/>
+            <param name="retry_hw" value="$(var retry_hw)"/>
+            <param name="ptp_profile" value="$(var ptp_profile)"/>
+            <param name="ptp_domain" value="$(var ptp_domain)"/>
+            <param name="ptp_transport_type" value="$(var ptp_transport_type)"/>
+            <param name="ptp_switch_type" value="$(var ptp_switch_type)"/>
+
+            <param name="diag_span" value="$(var diag_span)"/>
+        </composable_node>
+    </node_container>
 </launch>
diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp
@@ -86,118 +86,62 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options)
       std::static_pointer_cast<drivers::CalibrationConfigurationBase>(calibration_cfg_ptr_),
       std::static_pointer_cast<drivers::HesaiCorrection>(correction_cfg_ptr_));
 
-    RCLCPP_DEBUG(this->get_logger(), "Starting stream");
-    StreamStart();
-    hw_interface_.RegisterScanCallback(
-      std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1));
-
-  RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Wrapper=" << wrapper_status_);
-  rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
-  auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10),
-                         qos_profile);
-
-  packet_pub_ = create_publisher<nebula_msgs::msg::NebulaPacket>(
-    "hesai_packets", rclcpp::SensorDataQoS());
-
-  packet_sub_ = create_subscription<nebula_msgs::msg::NebulaPacket>(
-    "hesai_packets", rclcpp::SensorDataQoS(), [](nebula_msgs::msg::NebulaPacket::UniquePtr){});
-
-  nebula_points_pub_ =
-    this->create_publisher<sensor_msgs::msg::PointCloud2>("pandar_points", rclcpp::SensorDataQoS());
-  aw_points_base_pub_ =
-    this->create_publisher<sensor_msgs::msg::PointCloud2>("aw_points", rclcpp::SensorDataQoS());
-  aw_points_ex_pub_ =
-    this->create_publisher<sensor_msgs::msg::PointCloud2>("aw_points_ex", rclcpp::SensorDataQoS());
-}
-
-//hwmon
-{
-cbg_r_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant);
-  cbg_m_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-  cbg_m2_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
-  if (Status::OK != interface_status_) {
-    RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_);
-    return;
+    RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Wrapper=" << wrapper_status_);
+    rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
+    auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10),
+                          qos_profile);
+
+    packet_pub_ = create_publisher<nebula_msgs::msg::NebulaPacket>(
+      "hesai_packets", rclcpp::SensorDataQoS());
+
+    packet_sub_ = create_subscription<nebula_msgs::msg::NebulaPacket>(
+      "hesai_packets", rclcpp::SensorDataQoS(), std::bind(&HesaiRosWrapper::ProcessCloudPacket, this, std::placeholders::_1));
+
+    nebula_points_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(
+      "pandar_points", rclcpp::SensorDataQoS());
+    aw_points_base_pub_ =
+      this->create_publisher<sensor_msgs::msg::PointCloud2>("aw_points", rclcpp::SensorDataQoS());
+    aw_points_ex_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(
+      "aw_points_ex", rclcpp::SensorDataQoS());
   }
-  std::this_thread::sleep_for(std::chrono::milliseconds(this->delay_monitor_ms_));
 
-  message_sep = ": ";
-  not_supported_message = "Not supported";
-  error_message = "Error";
+  RCLCPP_DEBUG(this->get_logger(), "Starting stream");
+  StreamStart();
+  hw_interface_.RegisterScanCallback(
+    std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1));
 
-  switch (sensor_cfg_ptr_->sensor_model) {
-    case nebula::drivers::SensorModel::HESAI_PANDARXT32:
-    case nebula::drivers::SensorModel::HESAI_PANDARXT32M:
-    case nebula::drivers::SensorModel::HESAI_PANDARAT128:
-      temperature_names.emplace_back("Bottom circuit board T1");
-      temperature_names.emplace_back("Bottom circuit board T2");
-      temperature_names.emplace_back("Laser emitting board RT_L1(Internal)");
-      temperature_names.emplace_back("Laser emitting board RT_L2");
-      temperature_names.emplace_back("Receiving board RT_R");
-      temperature_names.emplace_back("Receiving board RT2");
-      temperature_names.emplace_back("Top circuit RT3");
-      temperature_names.emplace_back("Not used");
-      break;
-    case nebula::drivers::SensorModel::HESAI_PANDAR64:
-    case nebula::drivers::SensorModel::HESAI_PANDAR40P:
-    case nebula::drivers::SensorModel::HESAI_PANDAR40M:
-    case nebula::drivers::SensorModel::HESAI_PANDARQT64:
-    case nebula::drivers::SensorModel::HESAI_PANDARQT128:
-    case nebula::drivers::SensorModel::HESAI_PANDAR128_E3X:
-    case nebula::drivers::SensorModel::HESAI_PANDAR128_E4X:
-    default:
-      temperature_names.emplace_back("Bottom circuit RT1");
-      temperature_names.emplace_back("Bottom circuit RT2");
-      temperature_names.emplace_back("Internal Temperature");
-      temperature_names.emplace_back("Laser emitting board RT1");
-      temperature_names.emplace_back("Laser emitting board RT2");
-      temperature_names.emplace_back("Receiving board RT1");
-      temperature_names.emplace_back("Top circuit RT1");
-      temperature_names.emplace_back("Top circuit RT2");
-      break;
-  }
-
-  std::vector<std::thread> thread_pool{};
-  thread_pool.emplace_back([this] {
-    auto result = hw_interface_.GetInventory();
-    current_inventory.reset(new HesaiInventory(result));
-    current_inventory_time.reset(new rclcpp::Time(this->get_clock()->now()));
-    RCLCPP_INFO_STREAM(this->get_logger(), "HesaiInventory");
-    RCLCPP_INFO_STREAM(this->get_logger(), result);
-    info_model = result.get_str_model();
-    info_serial = std::string(result.sn.begin(), result.sn.end());
-    hw_interface_.SetTargetModel(result.model);
-    RCLCPP_INFO_STREAM(this->get_logger(), "Model:" << info_model);
-    RCLCPP_INFO_STREAM(this->get_logger(), "Serial:" << info_serial);
-    InitializeHesaiDiagnostics();
-  });
-  for (std::thread & th : thread_pool) {
-    th.join();
-  }
+  InitializeHwMonitor(*sensor_cfg_ptr_);
 
   set_param_res_ = this->add_on_set_parameters_callback(
     std::bind(&HesaiRosWrapper::paramCallback, this, std::placeholders::_1));
 }
 
 void HesaiRosWrapper::ReceiveCloudPacketCallback(const std::vector<uint8_t> & packet)
 {
-  auto t_received = std::chrono::high_resolution_clock::now().time_since_epoch().count();
-  auto msg_ptr = std::make_unique<nebula_msgs::msg::NebulaPacket>(packet.size());
-  msg_ptr->stamp.sec = t_received / 1'000'000'000;
-  msg_ptr->stamp.nanosec = t_received % 1'000'000'000;
-  // TODO(mojomex) this copy could be avoided if transport_drivers would give us a non-const vector
-  std::copy(packet.begin(), packet.end(), msg_ptr->data.begin());
-  packet_pub_->publish(std::move(msg_ptr));
-
-  //todo
   // Driver is not initialized yet
   if (!driver_ptr_) {
     return;
   }
 
+  const auto now = std::chrono::system_clock::now();
+  const auto timestamp_ns =
+    std::chrono::duration_cast<std::chrono::nanoseconds>(now.time_since_epoch()).count();
+
+  auto msg_ptr = std::make_unique<nebula_msgs::msg::NebulaPacket>();
+  msg_ptr->stamp.sec = static_cast<int>(timestamp_ns / 1'000'000'000);
+  msg_ptr->stamp.nanosec = static_cast<int>(timestamp_ns % 1'000'000'000);
+  std::copy(packet.begin(), packet.end(), std::back_inserter(msg_ptr->data));
+
+  packet_pub_->publish(std::move(msg_ptr));
+}
+
+void HesaiRosWrapper::ProcessCloudPacket(std::unique_ptr<nebula_msgs::msg::NebulaPacket> packet_msg)
+{
+  std::lock_guard lock(mtx_decode_);
+
   auto t_start = std::chrono::high_resolution_clock::now();
   std::tuple<nebula::drivers::NebulaPointCloudPtr, double> pointcloud_ts =
-    driver_ptr_->ParseCloudPacket(packet);
+    driver_ptr_->ParseCloudPacket(packet_msg->data);
   nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts);
 
   auto t_end = std::chrono::high_resolution_clock::now();