diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index b56fd03c5..2e7ac2093 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -620,7 +620,7 @@ struct FilterStatusPacket #pragma pack(pop) -struct PointARS548Detection +struct EIGEN_ALIGN16 PointARS548Detection { PCL_ADD_POINT4D; float azimuth; @@ -639,11 +639,11 @@ struct PointARS548Detection uint16_t object_id; uint8_t ambiguity_flag; EIGEN_MAKE_ALIGNED_OPERATOR_NEW -} EIGEN_ALIGN16; +}; // Note we only use a subset of the data since POINT_CLOUD_REGISTER_POINT_STRUCT has a limit in the // number of fields -struct PointARS548Object +struct EIGEN_ALIGN16 PointARS548Object { PCL_ADD_POINT4D; uint32_t id; @@ -664,7 +664,7 @@ struct PointARS548Object float shape_width_edge_mean; float dynamics_orientation_rate_mean; EIGEN_MAKE_ALIGNED_OPERATOR_NEW -} EIGEN_ALIGN16; +}; } // namespace continental_ars548 } // namespace drivers diff --git a/nebula_common/include/nebula_common/point_types.hpp b/nebula_common/include/nebula_common/point_types.hpp index 6f3feabe9..260b7b9db 100644 --- a/nebula_common/include/nebula_common/point_types.hpp +++ b/nebula_common/include/nebula_common/point_types.hpp @@ -8,13 +8,13 @@ namespace nebula { namespace drivers { -struct PointXYZIR +struct EIGEN_ALIGN16 PointXYZIR { PCL_ADD_POINT4D; float intensity; uint16_t ring; EIGEN_MAKE_ALIGNED_OPERATOR_NEW -} EIGEN_ALIGN16; +}; struct PointXYZICATR { @@ -43,7 +43,7 @@ struct PointXYZIRCAEDT std::uint32_t time_stamp; }; -struct PointXYZIRADT +struct EIGEN_ALIGN16 PointXYZIRADT { PCL_ADD_POINT4D; float intensity; @@ -53,7 +53,7 @@ struct PointXYZIRADT uint8_t return_type; double time_stamp; EIGEN_MAKE_ALIGNED_OPERATOR_NEW -} EIGEN_ALIGN16; +}; using NebulaPoint = PointXYZIRCAEDT; using NebulaPointPtr = std::shared_ptr; diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp index 2deeb7a28..e9a4b5031 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp @@ -578,6 +578,14 @@ Status ContinentalARS548HwInterface::SetYawRate(float yaw_rate) return Status::OK; } +Status ContinentalARS548HwInterface::CheckAndSetConfig() +{ + RCLCPP_ERROR( + *parent_node_logger, + "This functionality is not yet implemented. Sensor is probably out of sync with config now."); + return Status::ERROR_1; +} + void ContinentalARS548HwInterface::SetLogger(std::shared_ptr logger) { parent_node_logger = logger; diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp index 58cd25f09..8120a871b 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp @@ -406,6 +406,14 @@ Status MultiContinentalARS548HwInterface::SetYawRate(float yaw_rate) return Status::OK; } +Status MultiContinentalARS548HwInterface::CheckAndSetConfig() +{ + RCLCPP_ERROR( + *parent_node_logger, + "This functionality is not yet implemented. Sensor is probably out of sync with config now."); + return Status::ERROR_1; +} + void MultiContinentalARS548HwInterface::SetLogger(std::shared_ptr logger) { parent_node_logger = logger;