From a70fe6f09062a9f3a006ebf69fdbf64334ed2c7c Mon Sep 17 00:00:00 2001
From: amc-nu <abraham.monrroy@gmail.com>
Date: Thu, 14 Sep 2023 11:23:03 +0900
Subject: [PATCH] Velodyne. Allow setup_sensor False

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>
---
 .../nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp  | 2 --
 .../src/velodyne/velodyne_hw_interface_ros_wrapper.cpp       | 5 ++++-
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp
index f3e01eb42..b8f86f266 100644
--- a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp
+++ b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp
@@ -29,8 +29,6 @@ Status VelodyneHwInterface::InitializeSensorConfiguration(
 Status VelodyneHwInterface::SetSensorConfiguration(
   std::shared_ptr<SensorConfigurationBase> sensor_configuration)
 {
-  InitializeSensorConfiguration(sensor_configuration);
-
   VelodyneStatus status = CheckAndSetConfigBySnapshotAsync();
   Status rt = status;
   return rt;
diff --git a/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp
index 400fec791..d70a5e045 100644
--- a/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp
+++ b/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp
@@ -23,9 +23,12 @@ VelodyneHwInterfaceRosWrapper::VelodyneHwInterfaceRosWrapper(const rclcpp::NodeO
   RCLCPP_INFO_STREAM(this->get_logger(), "Initialize sensor_configuration");
   std::shared_ptr<drivers::SensorConfigurationBase> sensor_cfg_ptr =
     std::make_shared<drivers::VelodyneSensorConfiguration>(sensor_configuration_);
-  RCLCPP_INFO_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration");
+  RCLCPP_INFO_STREAM(this->get_logger(), "hw_interface_.InitializeSensorConfiguration");
+  hw_interface_.InitializeSensorConfiguration(
+    std::static_pointer_cast<drivers::SensorConfigurationBase>(sensor_cfg_ptr));
 
   if (this->setup_sensor) {
+    RCLCPP_INFO_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration");
     hw_interface_.SetSensorConfiguration(
       std::static_pointer_cast<drivers::SensorConfigurationBase>(sensor_cfg_ptr));
     updateParameters();