Velodyne. Allow setup_sensor False (autowarefoundation#75)

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>
knzo25 · Oct 2, 2023 · 3cee32f · 3cee32f
1 parent 99d3560
commit 3cee32f
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 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
@@ -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
@@ -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();