Skip to content

Commit

Permalink
refactor(nebula_ros): combine Hesai wrapper nodes into one
Browse files Browse the repository at this point in the history
This is step one of the single node refactoring of Nebula.
In this step, only the three Hesai wrapper nodes were combined into one, and no optimizations have been done that are made possible by this refactoring.

The next step is to do those optimizations (e.g. get rid of unnecessary pointcloud copying).
  • Loading branch information
mojomex committed Mar 21, 2024
1 parent 6c49621 commit dd1bbdc
Show file tree
Hide file tree
Showing 12 changed files with 1,476 additions and 2,034 deletions.
2 changes: 2 additions & 0 deletions nebula_common/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,8 @@ ament_auto_add_library(nebula_common SHARED
src/velodyne/velodyne_calibration_decoder.cpp
)

target_link_libraries(nebula_common yaml-cpp)

ament_auto_package()

# Set ROS_DISTRO macros
Expand Down
31 changes: 5 additions & 26 deletions nebula_ros/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -33,34 +33,13 @@ include_directories(
)
link_libraries(${YAML_CPP_LIBRARIES} ${PCL_LIBRARIES})
## Hesai
# Hw Interface
ament_auto_add_library(hesai_hw_ros_wrapper SHARED
src/hesai/hesai_hw_interface_ros_wrapper.cpp
)

rclcpp_components_register_node(hesai_hw_ros_wrapper
PLUGIN "HesaiHwInterfaceRosWrapper"
EXECUTABLE hesai_hw_interface_ros_wrapper_node
)

# Monitor
ament_auto_add_library(hesai_hw_monitor_ros_wrapper SHARED
src/hesai/hesai_hw_monitor_ros_wrapper.cpp
)

rclcpp_components_register_node(hesai_hw_monitor_ros_wrapper
PLUGIN "HesaiHwMonitorRosWrapper"
EXECUTABLE hesai_hw_monitor_ros_wrapper_node
)

# DriverDecoder
ament_auto_add_library(hesai_driver_ros_wrapper SHARED
src/hesai/hesai_decoder_ros_wrapper.cpp
ament_auto_add_library(hesai_ros_wrapper SHARED
src/hesai/hesai_ros_wrapper.cpp
)

rclcpp_components_register_node(hesai_driver_ros_wrapper
PLUGIN "HesaiDriverRosWrapper"
EXECUTABLE hesai_driver_ros_wrapper_node
rclcpp_components_register_node(hesai_ros_wrapper
PLUGIN "HesaiRosWrapper"
EXECUTABLE hesai_ros_wrapper_node
)

## Velodyne
Expand Down
68 changes: 68 additions & 0 deletions nebula_ros/include/nebula_ros/common/nebula_ros_wrapper_base.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,68 @@
#pragma once

#include "nebula_common/nebula_common.hpp"
#include "nebula_common/nebula_status.hpp"

#include <diagnostic_updater/diagnostic_updater.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_components/register_node_macro.hpp>

#include <sensor_msgs/msg/point_cloud2.hpp>

#include <string>
#include <vector>

namespace nebula
{
namespace ros
{
/// @brief Base class for ros wrapper of each sensor driver
class NebulaRosWrapperBase
{
public:
NebulaRosWrapperBase() = default;

NebulaRosWrapperBase(NebulaRosWrapperBase && c) = delete;
NebulaRosWrapperBase & operator=(NebulaRosWrapperBase && c) = delete;
NebulaRosWrapperBase(const NebulaRosWrapperBase & c) = delete;
NebulaRosWrapperBase & operator=(const NebulaRosWrapperBase & c) = delete;

/// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface)
/// @return Resulting status
virtual Status StreamStart() = 0;

/// @brief Stop point cloud streaming (not used)
/// @return Resulting status
virtual Status StreamStop() = 0;

/// @brief Shutdown (not used)
/// @return Resulting status
virtual Status Shutdown() = 0;

private:
/// @brief Virtual function for initializing hardware interface ros wrapper
/// @param sensor_configuration SensorConfiguration for this driver
/// @return Resulting status
virtual Status InitializeHwInterface(
const drivers::SensorConfigurationBase & sensor_configuration) = 0;

/// @brief Virtual function for initializing hardware monitor ros wrapper
/// @param sensor_configuration SensorConfiguration for this driver
/// @return Resulting status
virtual Status InitializeHwMonitor(
const drivers::SensorConfigurationBase & sensor_configuration) = 0;

/// @brief Virtual function for initializing ros wrapper
/// @param sensor_configuration SensorConfiguration for this driver
/// @param calibration_configuration CalibrationConfiguration for this driver
/// @return Resulting status
virtual Status InitializeCloudDriver(
std::shared_ptr<drivers::SensorConfigurationBase> sensor_configuration,
std::shared_ptr<drivers::CalibrationConfigurationBase> calibration_configuration) = 0;

/// @brief Point cloud publisher
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_pub_;
};

} // namespace ros
} // namespace nebula
106 changes: 0 additions & 106 deletions nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp

This file was deleted.

100 changes: 0 additions & 100 deletions nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp

This file was deleted.

Loading

0 comments on commit dd1bbdc

Please sign in to comment.