temporary progress

nebula_messages/nebula_msgs/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.14)
+cmake_minimum_required(VERSION 3.5)
 project(nebula_msgs)
 
 if (NOT CMAKE_CXX_STANDARD)
@@ -15,9 +15,10 @@ find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 
 rosidl_generate_interfaces(${PROJECT_NAME}
-        "msg/RawPacketStamped.msg"
+        "msg/NebulaPacket.msg"
+        "msg/NebulaPackets.msg"
         DEPENDENCIES
-        builtin_interfaces
+        std_msgs
         )
 
-ament_package()
+ament_package()

nebula_messages/nebula_msgs/msg/RawPacketStamped.msg → nebula_messages/nebula_msgs/msg/NebulaPacket.msg

@@ -1,2 +1,2 @@
 builtin_interfaces/Time stamp
-uint8[] data
+uint8[] data

nebula_messages/nebula_msgs/msg/NebulaPackets.msg

@@ -0,0 +1,2 @@
+std_msgs/Header   header
+NebulaPacket[]   packets

nebula_messages/nebula_msgs/package.xml

@@ -1,17 +1,19 @@
 <?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nebula_msgs</name>
-  <version>0.0.0</version>
-  <description>
-    ROS message definitions for timestamped UDP packets.
-  </description>
-  <maintainer email="[email protected]">Max Schmeller</maintainer>
+  <version>0.1.0</version>
+  <description>Generic sensor raw data messages for Nebula</description>
+  <maintainer email="[email protected]">Kenzo Lobos-Tsunekawa</maintainer>
+
   <license>Apache 2</license>
+  <author>Tier IV</author>
 
-  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
   <depend>builtin_interfaces</depend>
+  <depend>std_msgs</depend>
 
   <exec_depend>rosidl_default_runtime</exec_depend>
 
@@ -20,4 +22,4 @@
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>

nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp

@@ -15,7 +15,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_components/register_node_macro.hpp>
 
-#include "nebula_msgs/msg/raw_packet_stamped.hpp"
+#include "nebula_msgs/msg/nebula_packet.hpp"
 
 #include <boost/algorithm/string/join.hpp>
 #include <boost/asio.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
@@ -186,8 +190,10 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase
   std::shared_ptr<drivers::HesaiDriver> driver_ptr_;
   Status wrapper_status_;
 
-  rclcpp::Publisher<nebula_msgs::msg::RawPacketStamped>::SharedPtr packet_pub_;
-  rclcpp::Subscription<nebula_msgs::msg::RawPacketStamped>::SharedPtr packet_sub_;
+  std::mutex mtx_decode_;
+
+  rclcpp::Publisher<nebula_msgs::msg::NebulaPacket>::SharedPtr packet_pub_;
+  rclcpp::Subscription<nebula_msgs::msg::NebulaPacket>::SharedPtr packet_sub_;
 
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr nebula_points_pub_;
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr aw_points_ex_pub_;

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>

nebula_ros/src/hesai/hesai_ros_wrapper.cpp

@@ -86,18 +86,13 @@ 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_);
 
-    packet_pub_ = create_publisher<nebula_msgs::msg::RawPacketStamped>(
+    packet_pub_ = create_publisher<nebula_msgs::msg::NebulaPacket>(
       "hesai_packets", rclcpp::SensorDataQoS());
 
-    packet_sub_ = create_subscription<nebula_msgs::msg::RawPacketStamped>(
-      "hesai_packets", rclcpp::SensorDataQoS(), [](nebula_msgs::msg::RawPacketStamped::UniquePtr){});
+    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());
@@ -107,6 +102,11 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options)
       "aw_points_ex", rclcpp::SensorDataQoS());
   }
 
+  RCLCPP_DEBUG(this->get_logger(), "Starting stream");
+  StreamStart();
+  hw_interface_.RegisterScanCallback(
+    std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1));
+
   InitializeHwMonitor(*sensor_cfg_ptr_);
 
   set_param_res_ = this->add_on_set_parameters_callback(
@@ -115,23 +115,30 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options)
 
 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::RawPacketStamped>(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();