From f2ad48eaa3df33c59b4926051f28aa04ff30c213 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 23 May 2024 04:33:21 +0900 Subject: [PATCH] chore: deleted all nebula ros files Signed-off-by: Kenzo Lobos-Tsunekawa --- ...continental_srr520_decoder_ros_wrapper.hpp | 153 ----- ...nental_srr520_hw_interface_ros_wrapper.hpp | 161 ----- ...continental_srr520_decoder_ros_wrapper.cpp | 646 ------------------ ...nental_srr520_hw_interface_ros_wrapper.cpp | 384 ----------- 4 files changed, 1344 deletions(-) delete mode 100644 nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_ros_wrapper.hpp delete mode 100644 nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_ros_wrapper.hpp delete mode 100644 nebula_ros/src/continental/continental_srr520_decoder_ros_wrapper.cpp delete mode 100644 nebula_ros/src/continental/continental_srr520_hw_interface_ros_wrapper.cpp diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_ros_wrapper.hpp deleted file mode 100644 index 9aa21e685..000000000 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_ros_wrapper.hpp +++ /dev/null @@ -1,153 +0,0 @@ -// 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. - -#ifndef NEBULA_ContinentalSrr520DriverRosWrapper_H -#define NEBULA_ContinentalSrr520DriverRosWrapper_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Ros wrapper of continental radar ethernet driver -class ContinentalSrr520DriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperBase -{ - std::shared_ptr decoder_ptr_; - Status wrapper_status_; - - rclcpp::Subscription::SharedPtr packets_sub_; - - rclcpp::Publisher::SharedPtr - near_detection_list_pub_; - rclcpp::Publisher::SharedPtr - hrr_detection_list_pub_; - rclcpp::Publisher::SharedPtr object_list_pub_; - rclcpp::Publisher::SharedPtr status_pub_; - rclcpp::Publisher::SharedPtr near_detection_pointcloud_pub_; - rclcpp::Publisher::SharedPtr hrr_detection_pointcloud_pub_; - rclcpp::Publisher::SharedPtr object_pointcloud_pub_; - rclcpp::Publisher::SharedPtr near_scan_raw_pub_; - rclcpp::Publisher::SharedPtr hrr_scan_raw_pub_; - rclcpp::Publisher::SharedPtr objects_raw_pub_; - rclcpp::Publisher::SharedPtr objects_markers_pub_; - - std::unordered_set previous_ids_; - - std::shared_ptr - sensor_cfg_ptr_; - - drivers::continental_srr520::ContinentalSrr520HwInterface hw_interface_; - - /// @brief Initializing ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - Status InitializeDriver(std::shared_ptr sensor_configuration); - - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @param calibration_configuration Output of CalibrationConfiguration - /// @param correction_configuration Output of CorrectionConfiguration (for AT) - /// @return Resulting status - Status GetParameters( - drivers::continental_srr520::ContinentalSrr520SensorConfiguration & sensor_configuration); - - /// @brief Callback to process new Near ContinentalSrr520DetectionList from the driver - /// @param msg The new ContinentalSrr520DetectionList from the driver - void NearDetectionListCallback( - std::unique_ptr msg); - - /// @brief Callback to process new HRR ContinentalSrr520DetectionList from the driver - /// @param msg The new ContinentalSrr520DetectionList from the driver - void HRRDetectionListCallback( - std::unique_ptr msg); - - /// @brief Callback to process new ContinentalSrr520ObjectList from the driver - /// @param msg The new ContinentalSrr520ObjectList from the driver - void ObjectListCallback(std::unique_ptr msg); - - /// @brief Callback to process new DiagnosticArray from the driver - /// @param msg The new DiagnosticArray from the driver - void StatusCallback(std::unique_ptr msg); - -public: - explicit ContinentalSrr520DriverRosWrapper(const rclcpp::NodeOptions & options); - - /// @brief Callback for NebulaPackets subscriber - /// @param scan_msg Received NebulaPackets - void ReceivePacketsMsgCallback(const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg); - - /// @brief Get current status of this driver - /// @return Current status - Status GetStatus(); - - /// @brief Convert SRR520 detections to a pointcloud - /// @param msg The SRR520 detection list msg - /// @return Resulting detection pointcloud - pcl::PointCloud::Ptr - ConvertToPointcloud(const continental_msgs::msg::ContinentalSrr520DetectionList & msg); - - /// @brief Convert SRR520 objects to a pointcloud - /// @param msg The SRR520 object list msg - /// @return Resulting object pointcloud - pcl::PointCloud::Ptr ConvertToPointcloud( - const continental_msgs::msg::ContinentalSrr520ObjectList & msg); - - /// @brief Convert SRR520 detections to a standard RadarScan msg - /// @param msg The SRR520 detection list msg - /// @return Resulting RadarScan msg - radar_msgs::msg::RadarScan ConvertToRadarScan( - const continental_msgs::msg::ContinentalSrr520DetectionList & msg); - - /// @brief Convert SRR520 objects to a standard RadarTracks msg - /// @param msg The SRR520 object list msg - /// @return Resulting RadarTracks msg - radar_msgs::msg::RadarTracks ConvertToRadarTracks( - const continental_msgs::msg::ContinentalSrr520ObjectList & msg); - - /// @brief Convert SRR520 objects to a standard MarkerArray msg - /// @param msg The SRR520 object list msg - /// @return Resulting MarkerArray msg - visualization_msgs::msg::MarkerArray ConvertToMarkers( - const continental_msgs::msg::ContinentalSrr520ObjectList & msg); -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_ContinentalSrr520DriverRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_ros_wrapper.hpp deleted file mode 100644 index 5783fb051..000000000 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_ros_wrapper.hpp +++ /dev/null @@ -1,161 +0,0 @@ -// 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. - -#ifndef NEBULA_ContinentalSrr520HwInterfaceRosWrapper_H -#define NEBULA_ContinentalSrr520HwInterfaceRosWrapper_H - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/// @brief Hardware interface ros wrapper of continental radar ethernet driver -class ContinentalSrr520HwInterfaceRosWrapper final : public rclcpp::Node, - NebulaHwInterfaceWrapperBase -{ - drivers::continental_srr520::ContinentalSrr520HwInterface hw_interface_; - Status interface_status_; - - drivers::continental_srr520::ContinentalSrr520SensorConfiguration config_ptr_; - - /// @brief Received Continental Radar message publisher - rclcpp::Publisher::SharedPtr packets_pub_; - - message_filters::Subscriber odometry_sub_; - message_filters::Subscriber acceleration_sub_; - - using ExactTimeSyncPolicy = message_filters::sync_policies::ExactTime< - geometry_msgs::msg::TwistWithCovarianceStamped, geometry_msgs::msg::AccelWithCovarianceStamped>; - using ExactTimeSync = message_filters::Synchronizer; - std::shared_ptr sync_ptr_; - rclcpp::TimerBase::SharedPtr sync_timer_; - - bool standstill_{true}; - - rclcpp::Service::SharedPtr - configure_sensor_service_server_; - - /// @brief Initializing hardware interface ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwInterface( - const drivers::SensorConfigurationBase & sensor_configuration) override; - /// @brief Callback for receiving NebulaPackets - /// @param packets_buffer Received NebulaPackets - void ReceivePacketsDataCallback(std::unique_ptr packets_buffer); - - /// @brief Callback to send the odometry information to the radar device - /// @param odometry_msg The odometry message - /// @param acceleration_msg The acceleration message - void dynamicsCallback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr & odometry_msg, - const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr & acceleration_msg); - - /// @brief Service callback to set the new sensor mounting position - /// @param request Empty service request - /// @param response Empty service response - void ConfigureSensorRequestCallback( - const std::shared_ptr - request, - const std::shared_ptr - response); - - /// @brief Method periodically called to initiate the sensor synchronization mechanism - void syncTimerCallback(); - -public: - explicit ContinentalSrr520HwInterfaceRosWrapper(const rclcpp::NodeOptions & options); - ~ContinentalSrr520HwInterfaceRosWrapper() noexcept override; - - /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) - /// @return Resulting status - Status StreamStart() override; - /// @brief Stop point cloud streaming (not used) - /// @return Resulting status - Status StreamStop() override; - /// @brief Shutdown (not used) - /// @return Resulting status - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters( - drivers::continental_srr520::ContinentalSrr520SensorConfiguration & sensor_configuration); - -private: - std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - - /// @brief Updating rclcpp parameter - /// @return SetParametersResult - std::vector updateParameters(); -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_ContinentalSrr520HwInterfaceRosWrapper_H diff --git a/nebula_ros/src/continental/continental_srr520_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_decoder_ros_wrapper.cpp deleted file mode 100644 index 9184a0cfd..000000000 --- a/nebula_ros/src/continental/continental_srr520_decoder_ros_wrapper.cpp +++ /dev/null @@ -1,646 +0,0 @@ -// 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. - -#include - -#include - -namespace nebula -{ -namespace ros -{ -ContinentalSrr520DriverRosWrapper::ContinentalSrr520DriverRosWrapper( - const rclcpp::NodeOptions & options) -: rclcpp::Node("continental_srr520_driver_ros_wrapper", options), hw_interface_() -{ - drivers::continental_srr520::ContinentalSrr520SensorConfiguration sensor_configuration; - - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - - wrapper_status_ = GetParameters(sensor_configuration); - if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); - return; - } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); - - sensor_cfg_ptr_ = - std::make_shared( - sensor_configuration); - - wrapper_status_ = InitializeDriver( - std::const_pointer_cast( - sensor_cfg_ptr_)); - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); - packets_sub_ = create_subscription( - "nebula_packets", rclcpp::SensorDataQoS(), - std::bind( - &ContinentalSrr520DriverRosWrapper::ReceivePacketsMsgCallback, this, std::placeholders::_1)); - - near_detection_list_pub_ = - this->create_publisher( - "near_continental_detections", rclcpp::SensorDataQoS()); - hrr_detection_list_pub_ = - this->create_publisher( - "hrr_continental_detections", rclcpp::SensorDataQoS()); - object_list_pub_ = this->create_publisher( - "continental_objects", rclcpp::SensorDataQoS()); - status_pub_ = this->create_publisher("diagnostics", 10); - - near_detection_pointcloud_pub_ = this->create_publisher( - "near_detection_points", rclcpp::SensorDataQoS()); - hrr_detection_pointcloud_pub_ = this->create_publisher( - "hrr_detection_points", rclcpp::SensorDataQoS()); - object_pointcloud_pub_ = - this->create_publisher("object_points", rclcpp::SensorDataQoS()); - - near_scan_raw_pub_ = - this->create_publisher("near_scan_raw", rclcpp::SensorDataQoS()); - hrr_scan_raw_pub_ = - this->create_publisher("hrr_scan_raw", rclcpp::SensorDataQoS()); - - objects_raw_pub_ = - this->create_publisher("objects_raw", rclcpp::SensorDataQoS()); - - objects_markers_pub_ = - this->create_publisher("marker_array", 10); -} - -void ContinentalSrr520DriverRosWrapper::ReceivePacketsMsgCallback( - const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg) -{ - decoder_ptr_->ProcessPackets(*scan_msg); -} - -Status ContinentalSrr520DriverRosWrapper::InitializeDriver( - std::shared_ptr sensor_configuration) -{ - decoder_ptr_ = std::make_shared( - std::static_pointer_cast( - sensor_configuration)); - - decoder_ptr_->RegisterNearDetectionListCallback(std::bind( - &ContinentalSrr520DriverRosWrapper::NearDetectionListCallback, this, std::placeholders::_1)); - decoder_ptr_->RegisterHRRDetectionListCallback(std::bind( - &ContinentalSrr520DriverRosWrapper::HRRDetectionListCallback, this, std::placeholders::_1)); - decoder_ptr_->RegisterObjectListCallback( - std::bind(&ContinentalSrr520DriverRosWrapper::ObjectListCallback, this, std::placeholders::_1)); - decoder_ptr_->RegisterStatusCallback( - std::bind(&ContinentalSrr520DriverRosWrapper::StatusCallback, this, std::placeholders::_1)); - - return Status::OK; -} - -Status ContinentalSrr520DriverRosWrapper::GetStatus() -{ - return wrapper_status_; -} - -Status ContinentalSrr520DriverRosWrapper::GetParameters( - drivers::continental_srr520::ContinentalSrr520SensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", descriptor); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("interface", descriptor); - sensor_configuration.interface = this->get_parameter("interface").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("receiver_timeout_sec", descriptor); - sensor_configuration.receiver_timeout_sec = - this->get_parameter("receiver_timeout_sec").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sender_timeout_sec", descriptor); - sensor_configuration.sender_timeout_sec = this->get_parameter("sender_timeout_sec").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("filters", descriptor); - sensor_configuration.filters = this->get_parameter("filters").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("use_bus_time", descriptor); - sensor_configuration.use_bus_time = this->get_parameter("use_bus_time").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_wheelbase", descriptor); - sensor_configuration.configuration_vehicle_wheelbase = - static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - - if (sensor_configuration.frame_id.empty()) { - return Status::SENSOR_CONFIG_ERROR; - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void ContinentalSrr520DriverRosWrapper::NearDetectionListCallback( - std::unique_ptr msg) -{ - if ( - near_detection_pointcloud_pub_->get_subscription_count() > 0 || - near_detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) { - const auto detection_pointcloud_ptr = ConvertToPointcloud(*msg); - auto detection_pointcloud_msg_ptr = std::make_unique(); - pcl::toROSMsg(*detection_pointcloud_ptr, *detection_pointcloud_msg_ptr); - - detection_pointcloud_msg_ptr->header = msg->header; - near_detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr)); - } - - if ( - near_scan_raw_pub_->get_subscription_count() > 0 || - near_scan_raw_pub_->get_intra_process_subscription_count() > 0) { - auto radar_scan_msg = ConvertToRadarScan(*msg); - radar_scan_msg.header = msg->header; - near_scan_raw_pub_->publish(std::move(radar_scan_msg)); - } - - if ( - near_detection_list_pub_->get_subscription_count() > 0 || - near_detection_list_pub_->get_intra_process_subscription_count() > 0) { - near_detection_list_pub_->publish(std::move(msg)); - } -} - -void ContinentalSrr520DriverRosWrapper::HRRDetectionListCallback( - std::unique_ptr msg) -{ - if ( - hrr_detection_pointcloud_pub_->get_subscription_count() > 0 || - hrr_detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) { - const auto detection_pointcloud_ptr = ConvertToPointcloud(*msg); - auto detection_pointcloud_msg_ptr = std::make_unique(); - pcl::toROSMsg(*detection_pointcloud_ptr, *detection_pointcloud_msg_ptr); - - detection_pointcloud_msg_ptr->header = msg->header; - hrr_detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr)); - } - - if ( - hrr_scan_raw_pub_->get_subscription_count() > 0 || - hrr_scan_raw_pub_->get_intra_process_subscription_count() > 0) { - auto radar_scan_msg = ConvertToRadarScan(*msg); - radar_scan_msg.header = msg->header; - hrr_scan_raw_pub_->publish(std::move(radar_scan_msg)); - } - - if ( - hrr_detection_list_pub_->get_subscription_count() > 0 || - hrr_detection_list_pub_->get_intra_process_subscription_count() > 0) { - hrr_detection_list_pub_->publish(std::move(msg)); - } -} - -void ContinentalSrr520DriverRosWrapper::ObjectListCallback( - std::unique_ptr msg) -{ - if ( - object_pointcloud_pub_->get_subscription_count() > 0 || - object_pointcloud_pub_->get_intra_process_subscription_count() > 0) { - const auto object_pointcloud_ptr = ConvertToPointcloud(*msg); - auto object_pointcloud_msg_ptr = std::make_unique(); - pcl::toROSMsg(*object_pointcloud_ptr, *object_pointcloud_msg_ptr); - - object_pointcloud_msg_ptr->header = msg->header; - object_pointcloud_pub_->publish(std::move(object_pointcloud_msg_ptr)); - } - - if ( - objects_raw_pub_->get_subscription_count() > 0 || - objects_raw_pub_->get_intra_process_subscription_count() > 0) { - auto objects_raw_msg = ConvertToRadarTracks(*msg); - objects_raw_msg.header = msg->header; - objects_raw_pub_->publish(std::move(objects_raw_msg)); - } - - if ( - objects_markers_pub_->get_subscription_count() > 0 || - objects_markers_pub_->get_intra_process_subscription_count() > 0) { - auto marker_array_msg = ConvertToMarkers(*msg); - objects_markers_pub_->publish(std::move(marker_array_msg)); - } - - if ( - object_list_pub_->get_subscription_count() > 0 || - object_list_pub_->get_intra_process_subscription_count() > 0) { - object_list_pub_->publish(std::move(msg)); - } -} - -void ContinentalSrr520DriverRosWrapper::StatusCallback( - std::unique_ptr status_msg_ptr) -{ - status_pub_->publish(std::move(status_msg_ptr)); -} - -pcl::PointCloud::Ptr -ContinentalSrr520DriverRosWrapper::ConvertToPointcloud( - const continental_msgs::msg::ContinentalSrr520DetectionList & msg) -{ - pcl::PointCloud::Ptr output_pointcloud( - new pcl::PointCloud); - output_pointcloud->reserve(msg.detections.size()); - - nebula::drivers::continental_srr520::PointSrr520Detection point{}; - for (const auto & detection : msg.detections) { - point.x = std::cos(detection.azimuth_angle) * detection.range; - point.y = std::sin(detection.azimuth_angle) * detection.range; - point.z = 0.f; - - point.azimuth = detection.azimuth_angle; - point.range = detection.range; - point.range_rate = detection.range_rate; - point.rcs = detection.rcs; - point.snr = detection.snr; - point.pdh00 = detection.pdh00; - point.pdh01 = detection.pdh01; - point.pdh02 = detection.pdh02; - point.pdh03 = detection.pdh03; - point.pdh04 = detection.pdh04; - point.pdh05 = detection.pdh05; - - output_pointcloud->points.emplace_back(point); - } - - output_pointcloud->height = 1; - output_pointcloud->width = output_pointcloud->points.size(); - return output_pointcloud; -} - -pcl::PointCloud::Ptr -ContinentalSrr520DriverRosWrapper::ConvertToPointcloud( - const continental_msgs::msg::ContinentalSrr520ObjectList & msg) -{ - pcl::PointCloud::Ptr output_pointcloud( - new pcl::PointCloud); - output_pointcloud->reserve(msg.objects.size()); - - nebula::drivers::continental_srr520::PointSrr520Object point{}; - for (const auto & object : msg.objects) { - point.x = object.dist_x; - point.y = object.dist_y; - point.z = 0.f; - - point.id = object.object_id; - point.age = object.life_cycles; - - point.box_length = object.box_length; - point.box_width = object.box_width; - - point.object_status = object.object_status; - point.orientation = object.orientation; - point.rcs = object.rcs; - point.score = object.score; - point.dynamics_abs_vel_x = object.v_abs_x; - point.dynamics_abs_vel_y = object.v_abs_y; - point.dynamics_abs_acc_x = object.a_abs_x; - point.dynamics_abs_acc_y = object.a_abs_y; - - output_pointcloud->points.emplace_back(point); - } - - output_pointcloud->height = 1; - output_pointcloud->width = output_pointcloud->points.size(); - return output_pointcloud; -} - -radar_msgs::msg::RadarScan ContinentalSrr520DriverRosWrapper::ConvertToRadarScan( - const continental_msgs::msg::ContinentalSrr520DetectionList & msg) -{ - radar_msgs::msg::RadarScan output_msg; - output_msg.header = msg.header; - output_msg.returns.reserve(msg.detections.size()); - - radar_msgs::msg::RadarReturn return_msg; - for (const auto & detection : msg.detections) { - if (detection.pdh00 > 0 || detection.pdh00 > 1 || detection.pdh02 > 0) { - continue; - } - - return_msg.range = detection.range; - return_msg.azimuth = detection.azimuth_angle; - return_msg.elevation = 0.f; - return_msg.doppler_velocity = detection.range_rate; - return_msg.amplitude = detection.rcs; - output_msg.returns.emplace_back(return_msg); - } - - return output_msg; -} - -radar_msgs::msg::RadarTracks ContinentalSrr520DriverRosWrapper::ConvertToRadarTracks( - const continental_msgs::msg::ContinentalSrr520ObjectList & msg) -{ - radar_msgs::msg::RadarTracks output_msg; - output_msg.tracks.reserve(msg.objects.size()); - output_msg.header = msg.header; - - constexpr int16_t UNKNOWN_ID = 32000; - constexpr float INVALID_COVARIANCE = 1e6; - - radar_msgs::msg::RadarTrack track_msg; - for (const auto & object : msg.objects) { - if (!object.box_valid || object.object_status == 0) { - continue; - } - - track_msg.uuid.uuid[0] = static_cast(object.object_id & 0xff); - track_msg.uuid.uuid[1] = static_cast((object.object_id >> 8) & 0xff); - track_msg.uuid.uuid[2] = static_cast((object.object_id >> 16) & 0xff); - track_msg.uuid.uuid[3] = static_cast((object.object_id >> 24) & 0xff); - - track_msg.position.x = static_cast(object.dist_x); - track_msg.position.y = static_cast(object.dist_y); - track_msg.position.z = 0.0; - - track_msg.velocity.x = static_cast(object.v_abs_x); - track_msg.velocity.y = static_cast(object.v_abs_y); - track_msg.velocity.z = 0.0; - track_msg.acceleration.x = static_cast(object.a_abs_x); - track_msg.acceleration.y = static_cast(object.a_abs_y); - track_msg.acceleration.z = 0.0; - track_msg.size.x = object.box_length; - track_msg.size.y = object.box_width; - track_msg.size.z = 1.f; - - track_msg.classification = UNKNOWN_ID; - - track_msg.position_covariance[0] = object.dist_x_std * object.dist_x_std; - track_msg.position_covariance[1] = INVALID_COVARIANCE; - track_msg.position_covariance[2] = 0.f; - track_msg.position_covariance[3] = object.dist_y_std * object.dist_y_std; - track_msg.position_covariance[4] = 0.f; - track_msg.position_covariance[5] = INVALID_COVARIANCE; - - track_msg.velocity_covariance[0] = object.v_abs_x_std * object.v_abs_x_std; - track_msg.velocity_covariance[1] = INVALID_COVARIANCE; - track_msg.velocity_covariance[2] = 0.f; - track_msg.velocity_covariance[3] = object.v_abs_y_std * object.v_abs_y_std; - track_msg.velocity_covariance[4] = 0.f; - track_msg.velocity_covariance[5] = INVALID_COVARIANCE; - - track_msg.acceleration_covariance[0] = object.a_abs_x_std * object.a_abs_x_std; - track_msg.acceleration_covariance[1] = INVALID_COVARIANCE; - track_msg.acceleration_covariance[2] = 0.f; - track_msg.acceleration_covariance[3] = object.a_abs_y_std * object.a_abs_y_std; - track_msg.acceleration_covariance[4] = 0.f; - track_msg.acceleration_covariance[5] = INVALID_COVARIANCE; - - track_msg.size_covariance[0] = INVALID_COVARIANCE; - track_msg.size_covariance[1] = 0.f; - track_msg.size_covariance[2] = 0.f; - track_msg.size_covariance[3] = INVALID_COVARIANCE; - track_msg.size_covariance[4] = 0.f; - track_msg.size_covariance[5] = INVALID_COVARIANCE; - - output_msg.tracks.emplace_back(track_msg); - } - - return output_msg; -} - -visualization_msgs::msg::MarkerArray ContinentalSrr520DriverRosWrapper::ConvertToMarkers( - const continental_msgs::msg::ContinentalSrr520ObjectList & msg) -{ - visualization_msgs::msg::MarkerArray marker_array; - marker_array.markers.reserve(4 * msg.objects.size()); - - constexpr int LINE_STRIP_CORNERS_NUM = 17; - constexpr std::array, LINE_STRIP_CORNERS_NUM> cube_corners = { - {{{-1.0, -1.0, -1.0}}, - {{-1.0, -1.0, 1.0}}, - {{-1.0, 1.0, 1.0}}, - {{-1.0, 1.0, -1.0}}, - {{-1.0, -1.0, -1.0}}, - {{1.0, -1.0, -1.0}}, - {{1.0, -1.0, 1.0}}, - {{1.0, 1.0, 1.0}}, - {{1.0, 1.0, -1.0}}, - {{1.0, -1.0, -1.0}}, - {{-1.0, -1.0, -1.0}}, - {{-1.0, -1.0, 1.0}}, - {{1.0, -1.0, 1.0}}, - {{1.0, 1.0, 1.0}}, - {{-1.0, 1.0, 1.0}}, - {{-1.0, 1.0, -1.0}}, - {{1.0, 1.0, -1.0}}}}; - - constexpr int PALETTE_SIZE = 32; - constexpr std::array, PALETTE_SIZE> color_array = {{ - {{1.0, 0.0, 0.0}}, {{0.0, 1.0, 0.0}}, - {{0.0, 0.0, 1.0}}, // Red, Green, Blue - {{1.0, 1.0, 0.0}}, {{0.0, 1.0, 1.0}}, - {{1.0, 0.0, 1.0}}, // Yellow, Cyan, Magenta - {{1.0, 0.647, 0.0}}, {{0.749, 1.0, 0.0}}, - {{0.0, 0.502, 0.502}}, // Orange, Lime, Teal - {{0.502, 0.0, 0.502}}, {{1.0, 0.753, 0.796}}, - {{0.647, 0.165, 0.165}}, // Purple, Pink, Brown - {{0.502, 0.0, 0.0}}, {{0.502, 0.502, 0.0}}, - {{0.0, 0.0, 0.502}}, // Maroon, Olive, Navy - {{0.502, 0.502, 0.502}}, {{1.0, 0.4, 0.4}}, - {{0.4, 1.0, 0.4}}, // Grey, Light Red, Light Green - {{0.4, 0.4, 1.0}}, {{1.0, 1.0, 0.4}}, - {{0.4, 1.0, 1.0}}, // Light Blue, Light Yellow, Light Cyan - {{1.0, 0.4, 1.0}}, {{1.0, 0.698, 0.4}}, - {{0.698, 0.4, 1.0}}, // Light Magenta, Light Orange, Light Purple - {{1.0, 0.6, 0.8}}, {{0.71, 0.396, 0.114}}, - {{0.545, 0.0, 0.0}}, // Light Pink, Light Brown, Dark Red - {{0.0, 0.392, 0.0}}, {{0.0, 0.0, 0.545}}, - {{0.545, 0.545, 0.0}}, // Dark Green, Dark Blue, Dark Yellow - {{0.0, 0.545, 0.545}}, {{0.545, 0.0, 0.545}} // Dark Cyan, Dark Magenta - }}; - - std::unordered_set current_ids; - - for (const auto & object : msg.objects) { - if (!object.box_valid || object.object_status == 0) { - continue; - } - - const double half_length = 0.5 * object.box_length; - const double half_width = 0.5 * object.box_width; - constexpr double DEFAULT_HALF_SIZE = 1.0; - - const double & yaw = object.orientation; - current_ids.emplace(object.object_id); - - visualization_msgs::msg::Marker box_marker; - box_marker.header.frame_id = sensor_cfg_ptr_->base_frame; - box_marker.header.stamp = msg.header.stamp; - box_marker.ns = "boxes"; - box_marker.id = object.object_id; - box_marker.action = visualization_msgs::msg::Marker::ADD; - box_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - box_marker.lifetime = rclcpp::Duration::from_seconds(0); - box_marker.color.r = color_array[object.object_id % PALETTE_SIZE][0]; - box_marker.color.g = color_array[object.object_id % PALETTE_SIZE][1]; - box_marker.color.b = color_array[object.object_id % PALETTE_SIZE][2]; - box_marker.color.a = 1.0; - box_marker.scale.x = 0.1; - - box_marker.pose.position.x = object.dist_x; - box_marker.pose.position.y = object.dist_y; - box_marker.pose.position.z = DEFAULT_HALF_SIZE; - box_marker.pose.orientation.w = std::cos(0.5 * yaw); - box_marker.pose.orientation.z = std::sin(0.5 * yaw); - - for (const auto & corner : cube_corners) { - geometry_msgs::msg::Point p; - p.x = half_length * corner[0]; - p.y = half_width * corner[1]; - p.z = DEFAULT_HALF_SIZE * corner[2]; - box_marker.points.emplace_back(p); - } - - marker_array.markers.emplace_back(box_marker); - - visualization_msgs::msg::Marker text_marker = box_marker; - text_marker.ns = "object_age"; - text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - text_marker.color.r = 1.0; - text_marker.color.g = 1.0; - text_marker.color.b = 1.0; - text_marker.color.a = 1.0; - text_marker.scale.x = 0.3; - text_marker.scale.y = 0.3; - text_marker.scale.z = 0.3; - text_marker.pose.position.z += 0.5; - text_marker.text = "ID=" + std::to_string(object.object_id) + - " Age=" + std::to_string(object.life_cycles) + "ms"; - - marker_array.markers.emplace_back(text_marker); - - std::stringstream object_status_ss; - object_status_ss << std::fixed << std::setprecision(3) - << "ID=" << static_cast(object.object_id) << "\n" - << static_cast(object.box_length) << "/" - << static_cast(object.object_status); - - text_marker.ns = "object_status"; - text_marker.text = object_status_ss.str(); - - marker_array.markers.emplace_back(text_marker); - - std::stringstream object_dynamics_ss; - object_dynamics_ss << std::fixed << std::setprecision(3) - << "ID=" << static_cast(object.object_id) - << "\nyaw=" << object.orientation << "\nvx=" << object.v_abs_x - << "\nvy=" << object.v_abs_y << "\nax=" << object.a_abs_x - << "\nay=" << object.a_abs_y; - - text_marker.ns = "object_dynamics"; - text_marker.text = object_dynamics_ss.str(); - - marker_array.markers.emplace_back(text_marker); - } - - for (const auto & previous_id : previous_ids_) { - if (current_ids.find(previous_id) != current_ids.end()) { - continue; - } - - visualization_msgs::msg::Marker delete_marker; - delete_marker.header.frame_id = sensor_cfg_ptr_->base_frame; - delete_marker.header.stamp = msg.header.stamp; - delete_marker.ns = "boxes"; - delete_marker.id = previous_id; - delete_marker.action = visualization_msgs::msg::Marker::DELETE; - - marker_array.markers.push_back(delete_marker); - - delete_marker.ns = "object_age"; - marker_array.markers.push_back(delete_marker); - - delete_marker.ns = "object_status"; - marker_array.markers.push_back(delete_marker); - - delete_marker.ns = "object_dynamics"; - marker_array.markers.push_back(delete_marker); - } - - previous_ids_.clear(); - previous_ids_ = current_ids; - - return marker_array; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalSrr520DriverRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/continental/continental_srr520_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_hw_interface_ros_wrapper.cpp deleted file mode 100644 index 78b734e9c..000000000 --- a/nebula_ros/src/continental/continental_srr520_hw_interface_ros_wrapper.cpp +++ /dev/null @@ -1,384 +0,0 @@ -// 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. - -#include - -#include -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -ContinentalSrr520HwInterfaceRosWrapper::ContinentalSrr520HwInterfaceRosWrapper( - const rclcpp::NodeOptions & options) -: rclcpp::Node("continental_srr520_hw_interface_ros_wrapper", options), hw_interface_() -{ - if (mtx_config_.try_lock()) { - interface_status_ = GetParameters(sensor_configuration_); - mtx_config_.unlock(); - } - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - std::shared_ptr - sensor_cfg_ptr = - std::make_shared( - sensor_configuration_); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - hw_interface_.RegisterScanCallback(std::bind( - &ContinentalSrr520HwInterfaceRosWrapper::ReceivePacketsDataCallback, this, - std::placeholders::_1)); - packets_pub_ = this->create_publisher( - "nebula_packets", rclcpp::SensorDataQoS()); - - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&ContinentalSrr520HwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); - - StreamStart(); -} - -ContinentalSrr520HwInterfaceRosWrapper::~ContinentalSrr520HwInterfaceRosWrapper() -{ -} - -Status ContinentalSrr520HwInterfaceRosWrapper::StreamStart() -{ - using std::chrono_literals::operator""ms; - - if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.SensorInterfaceStart(); - } - - if (Status::OK == interface_status_) { - odometry_sub_.subscribe(this, "odometry_input"); - acceleration_sub_.subscribe(this, "acceleration_input"); - - sync_ptr_ = - std::make_shared(ExactTimeSyncPolicy(10), odometry_sub_, acceleration_sub_); - sync_ptr_->registerCallback(&ContinentalSrr520HwInterfaceRosWrapper::dynamicsCallback, this); - - configure_sensor_service_server_ = - this->create_service( - "configure_sensor", - std::bind( - &ContinentalSrr520HwInterfaceRosWrapper::ConfigureSensorRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - sync_timer_ = rclcpp::create_timer( - this, get_clock(), 100ms, - std::bind(&ContinentalSrr520HwInterfaceRosWrapper::syncTimerCallback, this)); - } - - return interface_status_; -} - -Status ContinentalSrr520HwInterfaceRosWrapper::StreamStop() -{ - hw_interface_.SensorInterfaceStop(); - return Status::OK; -} -Status ContinentalSrr520HwInterfaceRosWrapper::Shutdown() -{ - hw_interface_.SensorInterfaceStop(); - return Status::OK; -} - -Status ContinentalSrr520HwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think - // this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status ContinentalSrr520HwInterfaceRosWrapper::GetParameters( - drivers::continental_srr520::ContinentalSrr520SensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", descriptor); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("interface", descriptor); - sensor_configuration.interface = this->get_parameter("interface").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("receiver_timeout_sec", descriptor); - sensor_configuration.receiver_timeout_sec = - this->get_parameter("receiver_timeout_sec").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sender_timeout_sec", descriptor); - sensor_configuration.sender_timeout_sec = this->get_parameter("sender_timeout_sec").as_double(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("filters", descriptor); - sensor_configuration.filters = this->get_parameter("filters").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("use_bus_time", descriptor); - sensor_configuration.use_bus_time = this->get_parameter("use_bus_time").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_wheelbase", descriptor); - sensor_configuration.configuration_vehicle_wheelbase = - static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - - if (sensor_configuration.frame_id.empty()) { - return Status::SENSOR_CONFIG_ERROR; - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void ContinentalSrr520HwInterfaceRosWrapper::ReceivePacketsDataCallback( - std::unique_ptr scan_buffer) -{ - packets_pub_->publish(std::move(scan_buffer)); -} - -rcl_interfaces::msg::SetParametersResult ContinentalSrr520HwInterfaceRosWrapper::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(this->get_logger(), p); - RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), p); - - drivers::continental_srr520::ContinentalSrr520SensorConfiguration new_param{ - sensor_configuration_}; - RCLCPP_INFO_STREAM(this->get_logger(), new_param); - std::string sensor_model_str; - std::string return_mode_str; - - if ( - get_param(p, "sensor_model", sensor_model_str) | - get_param(p, "interface", new_param.interface) | - get_param(p, "receiver_timeout_sec", new_param.receiver_timeout_sec) | - get_param(p, "sender_timeout_sec", new_param.sender_timeout_sec) | - get_param(p, "frame_id", new_param.frame_id) | - get_param(p, "base_frame", new_param.base_frame) | get_param(p, "filters", new_param.filters) | - get_param(p, "use_bus_time", new_param.use_bus_time) | - get_param(p, "configuration_vehicle_wheelbase", new_param.configuration_vehicle_wheelbase)) { - if (0 < sensor_model_str.length()) - new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); - - sensor_configuration_ = new_param; - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared( - sensor_configuration_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - hw_interface_.CheckAndSetConfig(); - } - - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); - - return *result; -} - -void ContinentalSrr520HwInterfaceRosWrapper::dynamicsCallback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr & odometry_msg, - const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr & acceleration_msg) -{ - constexpr float speed_to_standstill = 0.5f; - constexpr float speed_to_moving = 2.f; - - if (standstill_ && std::abs(odometry_msg->twist.twist.linear.x) > speed_to_moving) { - standstill_ = false; - } else if (!standstill_ && std::abs(odometry_msg->twist.twist.linear.x) < speed_to_standstill) { - standstill_ = true; - } - - hw_interface_.SetVehicleDynamics( - acceleration_msg->accel.accel.linear.x, acceleration_msg->accel.accel.linear.y, - odometry_msg->twist.twist.angular.z, odometry_msg->twist.twist.linear.x, standstill_); -} - -void ContinentalSrr520HwInterfaceRosWrapper::ConfigureSensorRequestCallback( - const std::shared_ptr - request, - const std::shared_ptr - response) -{ - std::scoped_lock lock(mtx_config_); - - auto tf_buffer = std::make_unique(this->get_clock()); - auto tf_listener = std::make_unique(*tf_buffer); - - float longitudinal = request->longitudinal; - float lateral = request->lateral; - float vertical = request->vertical; - float yaw = request->yaw; - float vehicle_wheelbase = request->vehicle_wheelbase; - - if (vehicle_wheelbase < 0.f) { - RCLCPP_INFO( - this->get_logger(), - "Service vehicle_wheelbase is invalid. Falling back to configuration value (%.2f)", - sensor_configuration_.configuration_vehicle_wheelbase); - vehicle_wheelbase = sensor_configuration_.configuration_vehicle_wheelbase; - } - - if (request->autoconfigure_extrinsics) { - RCLCPP_INFO( - this->get_logger(), - "autoconfigure_extrinsics was set to true, so the mounting position will be derived from the " - "tfs"); - - geometry_msgs::msg::TransformStamped base_to_sensor_tf; - try { - base_to_sensor_tf = tf_buffer->lookupTransform( - sensor_configuration_.base_frame, sensor_configuration_.frame_id, rclcpp::Time(0), - rclcpp::Duration::from_seconds(0.5)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR( - this->get_logger(), "Could not obtain the transform from the base frame to %s (%s)", - sensor_configuration_.frame_id.c_str(), ex.what()); - response->success = false; - response->message = ex.what(); - return; - } - - const auto & quat = base_to_sensor_tf.transform.rotation; - geometry_msgs::msg::Vector3 rpy; - tf2::Matrix3x3(tf2::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(rpy.x, rpy.y, rpy.z); - - longitudinal = base_to_sensor_tf.transform.translation.x - vehicle_wheelbase; - lateral = base_to_sensor_tf.transform.translation.y; - vertical = base_to_sensor_tf.transform.translation.z; - yaw = rpy.z; - } - - yaw = std::min(std::max(yaw, -3.14159f), 3.14159f); - - auto result = hw_interface_.ConfigureSensor( - request->sensor_id, longitudinal, lateral, vertical, yaw, - longitudinal + 0.5 * vehicle_wheelbase, vehicle_wheelbase, request->cover_damping, - request->plug_bottom, request->reset_sensor_configuration); - - response->success = result == Status::OK; - response->message = (std::stringstream() << result).str(); -} - -void ContinentalSrr520HwInterfaceRosWrapper::syncTimerCallback() -{ - hw_interface_.SensorSync(); -} - -std::vector -ContinentalSrr520HwInterfaceRosWrapper::updateParameters() -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); - std::ostringstream os_sensor_model; - os_sensor_model << sensor_configuration_.sensor_model; - RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); - - auto results = set_parameters( - {rclcpp::Parameter("sensor_model", os_sensor_model.str()), - rclcpp::Parameter("interface", sensor_configuration_.interface), - rclcpp::Parameter("receiver_timeout_sec", sensor_configuration_.receiver_timeout_sec), - rclcpp::Parameter("sender_timeout_sec", sensor_configuration_.sender_timeout_sec), - rclcpp::Parameter("frame_id", sensor_configuration_.frame_id), - rclcpp::Parameter("base_frame", sensor_configuration_.base_frame), - rclcpp::Parameter("filters", sensor_configuration_.filters), - rclcpp::Parameter("use_bus_time", sensor_configuration_.use_bus_time), - rclcpp::Parameter( - "configuration_vehicle_wheelbase", sensor_configuration_.configuration_vehicle_wheelbase)}); - - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); - return results; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalSrr520HwInterfaceRosWrapper) -} // namespace ros -} // namespace nebula