feat(aeva): add ros wrapper

nebula_ros/CMakeLists.txt

@@ -215,11 +215,34 @@ rclcpp_components_register_node(continental_srr520_ros_wrapper
     EXECUTABLE continental_srr520_ros_wrapper_node
 )
 
+## Aeva
+add_library(aeva_ros_wrapper SHARED
+    src/aeva/aeva_ros_wrapper.cpp
+    src/aeva/hw_monitor_wrapper.cpp
+    src/common/parameter_descriptors.cpp
+)
+
+target_include_directories(aeva_ros_wrapper PUBLIC
+    ${nebula_decoders_INCLUDE_DIRS}
+    ${nebula_hw_interfaces_INCLUDE_DIRS}
+)
+
+target_link_libraries(aeva_ros_wrapper PUBLIC
+    nebula_decoders::nebula_decoders_aeva
+    nebula_hw_interfaces::nebula_hw_interfaces_aeva
+)
+
+rclcpp_components_register_node(aeva_ros_wrapper
+    PLUGIN "AevaRosWrapper"
+    EXECUTABLE aeva_ros_wrapper_node
+)
+
 install(TARGETS hesai_ros_wrapper EXPORT export_hesai_ros_wrapper)
 install(TARGETS velodyne_ros_wrapper EXPORT export_velodyne_ros_wrapper)
 install(TARGETS robosense_ros_wrapper EXPORT export_robosense_ros_wrapper)
 install(TARGETS continental_ars548_ros_wrapper EXPORT export_continental_ars548_ros_wrapper)
 install(TARGETS continental_srr520_ros_wrapper EXPORT export_continental_srr520_ros_wrapper)
+install(TARGETS aeva_ros_wrapper EXPORT export_aeva_ros_wrapper)
 install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME})
 
 if(BUILD_TESTING)
@@ -232,6 +255,7 @@ ament_export_targets(export_hesai_ros_wrapper)
 ament_export_targets(export_velodyne_ros_wrapper)
 ament_export_targets(export_robosense_ros_wrapper)
 ament_export_targets(export_continental_ars548_ros_wrapper)
+ament_export_targets(export_aeva_ros_wrapper)
 
 install(
   DIRECTORY config launch

nebula_ros/include/nebula_ros/aeva/aeva_ros_wrapper.hpp

@@ -0,0 +1,76 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include "nebula_ros/aeva/hw_monitor_wrapper.hpp"
+#include "nebula_ros/common/watchdog_timer.hpp"
+
+#include <ament_index_cpp/get_package_prefix.hpp>
+#include <nebula_common/aeva/config_types.hpp>
+#include <nebula_common/nebula_common.hpp>
+#include <nebula_common/nebula_status.hpp>
+#include <nebula_common/util/mt_queue.hpp>
+#include <nebula_decoders/nebula_decoders_aeva/aeva_aries2_decoder.hpp>
+#include <nebula_hw_interfaces/nebula_hw_interfaces_aeva/aeva_hw_interface.hpp>
+#include <nlohmann/json.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/subscription.hpp>
+#include <rclcpp_components/register_node_macro.hpp>
+
+#include <nebula_msgs/msg/nebula_packets.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+
+#include <boost/algorithm/string/join.hpp>
+#include <boost/asio.hpp>
+#include <boost/lexical_cast.hpp>
+
+#include <cstdint>
+#include <memory>
+#include <mutex>
+#include <optional>
+#include <vector>
+
+namespace nebula::ros
+{
+
+/// @brief Ros wrapper of hesai driver
+class AevaRosWrapper final : public rclcpp::Node
+{
+public:
+  explicit AevaRosWrapper(const rclcpp::NodeOptions & options);
+
+private:
+  Status declareAndGetSensorConfigParams();
+  Status validateAndSetConfig(std::shared_ptr<const drivers::aeva::Aeries2Config> & new_config);
+
+  void recordRawPacket(const std::vector<uint8_t> & bytes);
+
+  rclcpp::Publisher<nebula_msgs::msg::NebulaPackets>::SharedPtr packets_pub_{};
+  std::mutex mtx_current_scan_msg_;
+  nebula_msgs::msg::NebulaPackets::UniquePtr current_scan_msg_{};
+
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_pub_{};
+  std::shared_ptr<nebula::ros::WatchdogTimer> cloud_watchdog_;
+
+  rclcpp::Subscription<nebula_msgs::msg::NebulaPackets>::SharedPtr packets_sub_{};
+
+  std::shared_ptr<const drivers::aeva::Aeries2Config> sensor_cfg_ptr_;
+
+  std::optional<drivers::AevaHwInterface> hw_interface_;
+  std::optional<AevaHwMonitorWrapper> hw_monitor_;
+  drivers::AevaAries2Decoder decoder_{};
+};
+
+}  // namespace nebula::ros

nebula_ros/include/nebula_ros/aeva/hw_monitor_wrapper.hpp

@@ -0,0 +1,90 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <nebula_common/aeva/config_types.hpp>
+#include <nebula_common/hesai/hesai_common.hpp>
+#include <nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp>
+#include <nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp>
+#include <nlohmann/json.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/time.hpp>
+#include <rclcpp/timer.hpp>
+
+#include <boost/asio.hpp>
+#include <boost/lexical_cast.hpp>
+#include <boost/property_tree/ptree.hpp>
+
+#include <map>
+#include <memory>
+#include <mutex>
+#include <optional>
+#include <string>
+#include <vector>
+
+namespace nebula::ros
+{
+
+using nlohmann::json;
+
+class AevaHwMonitorWrapper
+{
+public:
+  AevaHwMonitorWrapper(
+    rclcpp::Node * const parent_node, std::shared_ptr<const drivers::aeva::Aeries2Config> config);
+
+  void onTelemetryFragment(const json & diff);
+
+  void onHealthCodes(std::vector<uint32_t> health_codes);
+
+private:
+  struct NodeTelemetry
+  {
+    json values;
+    rclcpp::Time last_update;
+  };
+
+  struct TelemetryState
+  {
+    std::map<std::string, NodeTelemetry> entries;
+  };
+
+  struct HealthState
+  {
+    std::vector<uint32_t> codes;
+    rclcpp::Time last_update;
+  };
+
+  void publishDiagnostics();
+
+  rclcpp::Logger logger_;
+  rclcpp::Node * const parent_node_;
+  std::shared_ptr<const drivers::aeva::Aeries2Config> config_;
+
+  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostics_pub_{};
+  rclcpp::TimerBase::SharedPtr diagnostics_pub_timer_{};
+  uint16_t diag_span_;
+
+  std::mutex mtx_hardware_id_;
+  std::optional<std::string> hardware_id_;
+
+  std::mutex mtx_telemetry_;
+  TelemetryState telemetry_;
+
+  std::mutex mtx_health_;
+  std::optional<HealthState> health_;
+};
+}  // namespace nebula::ros

nebula_ros/include/nebula_ros/common/nebula_packet_stream.hpp

@@ -0,0 +1,58 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include "nebula_hw_interfaces/nebula_hw_interfaces_common/connections/byte_stream.hpp"
+
+#include <rclcpp/node.hpp>
+#include <rclcpp/subscription.hpp>
+
+#include "nebula_msgs/msg/detail/nebula_packets__struct.hpp"
+
+#include <functional>
+#include <memory>
+#include <mutex>
+#include <utility>
+
+namespace nebula::ros
+{
+
+class NebulaPacketStream : public drivers::connections::ObservableByteStream
+{
+public:
+  NebulaPacketStream() = default;
+
+  void registerBytesCallback(callback_t callback) override
+  {
+    std::lock_guard lock(mtx_callback_);
+    callback_ = std::move(callback);
+  }
+
+  void onNebulaPackets(std::unique_ptr<nebula_msgs::msg::NebulaPackets> packets)
+  {
+    std::lock_guard lock(mtx_callback_);
+    if (!callback_) return;
+
+    for (auto & packet : packets->packets) {
+      callback_(packet.data);
+    }
+  }
+
+private:
+  std::mutex mtx_callback_{};
+  callback_t callback_{};
+};
+
+}  // namespace nebula::ros