From f8bea9a5b7fd7ad85580fbaaca83a431f60682fc Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 27 May 2024 15:47:22 +0900 Subject: [PATCH] chote: rebased and fixed rebse-related errors Signed-off-by: Kenzo Lobos-Tsunekawa --- .pre-commit-config.yaml | 2 +- README.md | 1 + build_depends.repos | 4 + .../include/nebula_common/nebula_common.hpp | 81 ++++++++++++++----- .../include/nebula_common/nebula_status.hpp | 15 ++++ nebula_decoders/CMakeLists.txt | 1 + .../decoders/continental_ars548_decoder.hpp | 21 +---- .../decoders/continental_ars548_decoder.cpp | 51 ++++++------ nebula_hw_interfaces/package.xml | 1 + .../continental_ars548_hw_interface.cpp | 22 ++--- .../continental_msgs/CMakeLists.txt | 4 + .../msg/ContinentalSrr520Detection.msg | 11 +++ .../msg/ContinentalSrr520DetectionList.msg | 10 +++ .../msg/ContinentalSrr520Object.msg | 21 +++++ .../msg/ContinentalSrr520ObjectList.msg | 12 +++ .../continental_srvs/CMakeLists.txt | 1 + .../ContinentalSrr520SetRadarParameters.srv | 15 ++++ nebula_ros/config/continental/SRR520.yaml | 15 ++++ 18 files changed, 217 insertions(+), 71 deletions(-) create mode 100644 nebula_messages/continental_msgs/msg/ContinentalSrr520Detection.msg create mode 100644 nebula_messages/continental_msgs/msg/ContinentalSrr520DetectionList.msg create mode 100644 nebula_messages/continental_msgs/msg/ContinentalSrr520Object.msg create mode 100644 nebula_messages/continental_msgs/msg/ContinentalSrr520ObjectList.msg create mode 100644 nebula_messages/continental_srvs/srv/ContinentalSrr520SetRadarParameters.srv create mode 100644 nebula_ros/config/continental/SRR520.yaml diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b4c2e9f2f..498a511ff 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -18,7 +18,7 @@ repos: args: [--markdown-linebreak-ext=md] - repo: https://github.com/igorshubovych/markdownlint-cli - rev: v0.40.0 + rev: v0.41.0 hooks: - id: markdownlint args: [-c, .markdownlint.yaml, --fix] diff --git a/README.md b/README.md index a509950ca..bc0745b09 100644 --- a/README.md +++ b/README.md @@ -99,6 +99,7 @@ Supported models, where sensor_model is the ROS param to be used at launch: | Velodyne | VLP-32 | VLP32 | VLP32.yaml | :warning: | | Velodyne | VLS-128 | VLS128 | VLS128.yaml | :warning: | | Continental | ARS548 | ARS548 | ARS548.yaml | :warning: | +| Continental | SRR520 | SRR520 | SRR520.yaml | :warning: | Test status:\ :heavy_check_mark:: complete\ diff --git a/build_depends.repos b/build_depends.repos index e9278685a..80e0d3165 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -4,3 +4,7 @@ repositories: type: git url: https://github.com/mojomex/transport_drivers version: mutable-buffer-in-udp-callback + ros2_socketcan: + type: git + url: https://github.com/knzo25/ros2_socketcan + version: feat/continental_fd \ No newline at end of file diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index a7deeae7c..6713fdd22 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -2,10 +2,7 @@ #define NEBULA_COMMON_H #include - #include - -#include #include #include #include @@ -327,9 +324,11 @@ enum class SensorModel { VELODYNE_HDL32, VELODYNE_VLP16, ROBOSENSE_HELIOS, + ROBOSENSE_BPEARL, ROBOSENSE_BPEARL_V3, ROBOSENSE_BPEARL_V4, - CONTINENTAL_ARS548 + CONTINENTAL_ARS548, + CONTINENTAL_SRR520 }; /// @brief not used? @@ -344,11 +343,24 @@ enum class datatype { FLOAT64 = 8 }; -enum class PtpProfile { IEEE_1588v2 = 0, IEEE_802_1AS, IEEE_802_1AS_AUTO, UNKNOWN_PROFILE }; +enum class PtpProfile { + IEEE_1588v2 = 0, + IEEE_802_1AS, + IEEE_802_1AS_AUTO, + UNKNOWN_PROFILE +}; -enum class PtpTransportType { UDP_IP = 0, L2, UNKNOWN_TRANSPORT }; +enum class PtpTransportType { + UDP_IP = 0, + L2, + UNKNOWN_TRANSPORT +}; -enum class PtpSwitchType { NON_TSN = 0, TSN, UNKNOWN_SWITCH }; +enum class PtpSwitchType { + NON_TSN = 0, + TSN, + UNKNOWN_SWITCH +}; /// @brief not used? struct PointField @@ -417,6 +429,9 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel case SensorModel::ROBOSENSE_HELIOS: os << "HELIOS"; break; + case SensorModel::ROBOSENSE_BPEARL: + os << "BPEARL"; + break; case SensorModel::ROBOSENSE_BPEARL_V3: os << "BPEARL V3.0"; break; @@ -426,6 +441,9 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel case SensorModel::CONTINENTAL_ARS548: os << "ARS548"; break; + case SensorModel::CONTINENTAL_SRR520: + os << "SRR520"; + break; case SensorModel::UNKNOWN: os << "Sensor Unknown"; break; @@ -448,6 +466,16 @@ struct EthernetSensorConfigurationBase : SensorConfigurationBase uint16_t data_port; }; +/// @brief Base struct for CAN-based Sensor configuration +struct CANSensorConfigurationBase : SensorConfigurationBase +{ + std::string interface; + float receiver_timeout_sec{}; + float sender_timeout_sec{}; + std::string filters{}; + bool use_bus_time{}; +}; + /// @brief Base struct for Lidar configuration struct LidarConfigurationBase : EthernetSensorConfigurationBase { @@ -483,6 +511,19 @@ inline std::ostream & operator<<(std::ostream & os, EthernetSensorConfigurationB return os; } +/// @brief Convert CANSensorConfigurationBase to string (Overloading the << operator) +/// @param os +/// @param arg +/// @return stream +inline std::ostream & operator<<(std::ostream & os, CANSensorConfigurationBase const & arg) +{ + os << (SensorConfigurationBase)(arg) + << ", Interface: " << arg.interface << ", ReceiverTimeoutSec: " << arg.receiver_timeout_sec + << ", SenderTimeoutSec: " << arg.sender_timeout_sec << ", Filters: " << arg.filters + << ", UseBusTime: " << arg.use_bus_time; + return os; +} + /// @brief Convert LidarConfigurationBase to string (Overloading the << operator) /// @param os /// @param arg @@ -526,10 +567,12 @@ inline SensorModel SensorModelFromString(const std::string & sensor_model) if (sensor_model == "VLP16") return SensorModel::VELODYNE_VLP16; // Robosense if (sensor_model == "Helios") return SensorModel::ROBOSENSE_HELIOS; - if (sensor_model == "Bpearl" || sensor_model == "Bpearl_V4") - return SensorModel::ROBOSENSE_BPEARL_V4; + if (sensor_model == "Bpearl") return SensorModel::ROBOSENSE_BPEARL; if (sensor_model == "Bpearl_V3") return SensorModel::ROBOSENSE_BPEARL_V3; + if (sensor_model == "Bpearl_V4") return SensorModel::ROBOSENSE_BPEARL_V4; + // Continental if (sensor_model == "ARS548") return SensorModel::CONTINENTAL_ARS548; + if (sensor_model == "SRR520") return SensorModel::CONTINENTAL_SRR520; return SensorModel::UNKNOWN; } @@ -571,12 +614,17 @@ inline std::string SensorModelToString(const SensorModel & sensor_model) // Robosense case SensorModel::ROBOSENSE_HELIOS: return "Helios"; + case SensorModel::ROBOSENSE_BPEARL: + return "Bpearl"; case SensorModel::ROBOSENSE_BPEARL_V3: return "Bpearl_V3"; case SensorModel::ROBOSENSE_BPEARL_V4: return "Bpearl_V4"; + // Continental case SensorModel::CONTINENTAL_ARS548: return "ARS548"; + case SensorModel::CONTINENTAL_SRR520: + return "SRR520"; default: return "UNKNOWN"; } @@ -602,9 +650,8 @@ inline PtpProfile PtpProfileFromString(const std::string & ptp_profile) { // Hesai auto tmp_str = ptp_profile; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { - return std::tolower(c); - }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), + [](unsigned char c){ return std::tolower(c); }); if (tmp_str == "1588v2") return PtpProfile::IEEE_1588v2; if (tmp_str == "802.1as") return PtpProfile::IEEE_802_1AS; if (tmp_str == "automotive") return PtpProfile::IEEE_802_1AS_AUTO; @@ -642,9 +689,8 @@ inline PtpTransportType PtpTransportTypeFromString(const std::string & transport { // Hesai auto tmp_str = transport_type; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { - return std::tolower(c); - }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), + [](unsigned char c){ return std::tolower(c); }); if (tmp_str == "udp") return PtpTransportType::UDP_IP; if (tmp_str == "l2") return PtpTransportType::L2; @@ -678,9 +724,8 @@ inline PtpSwitchType PtpSwitchTypeFromString(const std::string & switch_type) { // Hesai auto tmp_str = switch_type; - std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), [](unsigned char c) { - return std::tolower(c); - }); + std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(), + [](unsigned char c){ return std::tolower(c); }); if (tmp_str == "tsn") return PtpSwitchType::TSN; if (tmp_str == "non_tsn") return PtpSwitchType::NON_TSN; diff --git a/nebula_common/include/nebula_common/nebula_status.hpp b/nebula_common/include/nebula_common/nebula_status.hpp index f36d8b7d2..aff304f27 100644 --- a/nebula_common/include/nebula_common/nebula_status.hpp +++ b/nebula_common/include/nebula_common/nebula_status.hpp @@ -1,3 +1,17 @@ +// 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_STATUS_HPP #define NEBULA_STATUS_HPP @@ -14,6 +28,7 @@ struct Status enum Type { OK = 0, UDP_CONNECTION_ERROR, + CAN_CONNECTION_ERROR, SENSOR_CONFIG_ERROR, INVALID_SENSOR_MODEL, INVALID_ECHO_MODE, diff --git a/nebula_decoders/CMakeLists.txt b/nebula_decoders/CMakeLists.txt index 31385de6f..0096fbeee 100644 --- a/nebula_decoders/CMakeLists.txt +++ b/nebula_decoders/CMakeLists.txt @@ -55,6 +55,7 @@ ament_auto_add_library(nebula_decoders_robosense_info SHARED # Continental ament_auto_add_library(nebula_decoders_continental SHARED src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp + src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp ) if(BUILD_TESTING) diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp index b492c0197..c6eeb300f 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp @@ -51,21 +51,6 @@ class ContinentalArs548Decoder : public ContinentalPacketsDecoder /// @return Resulting flag bool ProcessPacket(std::unique_ptr packet_msg) override; - /// @brief Function for parsing detection lists - /// @param data - /// @return Resulting flag - bool ParseDetectionsListPacket(std::unique_ptr packet_msg); - - /// @brief Function for parsing object lists - /// @param data - /// @return Resulting flag - bool ParseObjectsListPacket(std::unique_ptr packet_msg); - - /// @brief Function for parsing sensor status messages - /// @param data - /// @return Resulting flag - bool ParseSensorStatusPacket(std::unique_ptr packet_msg); - /// @brief Register function to call when a new detection list is processed /// @param detection_list_callback /// @return Resulting status @@ -111,10 +96,10 @@ class ContinentalArs548Decoder : public ContinentalPacketsDecoder std::function msg)> detection_list_callback_{}; std::function msg)> - object_list_callback_; - std::function sensor_status_callback_; + object_list_callback_{}; + std::function sensor_status_callback_{}; std::function msg)> - nebula_packets_callback_; + nebula_packets_callback_{}; ContinentalArs548Status radar_status_{}; diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp index 07a1dc18a..9080db862 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp @@ -89,13 +89,13 @@ bool ContinentalArs548Decoder::ProcessPacket( return false; } - return ParseDetectionsListPacket(std::move(packet_msg)); + ParseDetectionsListPacket(*packet_msg); } else if (header.method_id.value() == OBJECT_LIST_METHOD_ID) { if (data.size() != OBJECT_LIST_UDP_PAYLOAD || header.length.value() != OBJECT_LIST_PDU_LENGTH) { return false; } - return ParseObjectsListPacket(std::move(packet_msg)); + ParseObjectsListPacket(*packet_msg); } else if (header.method_id.value() == SENSOR_STATUS_METHOD_ID) { if ( data.size() != SENSOR_STATUS_UDP_PAYLOAD || @@ -103,22 +103,31 @@ bool ContinentalArs548Decoder::ProcessPacket( return false; } - return ParseSensorStatusPacket(std::move(packet_msg)); + ParseSensorStatusPacket(*packet_msg); + } + + // Some messages are not parsed but are still sent to the user (e.g., filters) + if (nebula_packets_callback_) { + auto packets_msg = std::make_unique(); + packets_msg->packets.emplace_back(std::move(*packet_msg)); + packets_msg->header.stamp = packet_msg->stamp; + packets_msg->header.frame_id = config_ptr_->frame_id; + nebula_packets_callback_(std::move(packets_msg)); } return true; } bool ContinentalArs548Decoder::ParseDetectionsListPacket( - std::unique_ptr packet_msg) + const nebula_msgs::msg::NebulaPacket & packet_msg) { auto msg_ptr = std::make_unique(); auto & msg = *msg_ptr; DetectionListPacket detection_list; - assert(sizeof(DetectionListPacket) == packet_msg->data.size()); + assert(sizeof(DetectionListPacket) == packet_msg.data.size()); - std::memcpy(&detection_list, packet_msg->data.data(), sizeof(DetectionListPacket)); + std::memcpy(&detection_list, packet_msg.data.data(), sizeof(DetectionListPacket)); msg.header.frame_id = config_ptr_->frame_id; @@ -126,7 +135,7 @@ bool ContinentalArs548Decoder::ParseDetectionsListPacket( msg.header.stamp.nanosec = detection_list.stamp.timestamp_nanoseconds.value(); msg.header.stamp.sec = detection_list.stamp.timestamp_seconds.value(); } else { - msg.header.stamp = packet_msg->stamp; + msg.header.stamp = packet_msg.stamp; } msg.stamp_sync_status = detection_list.stamp.timestamp_sync_status; @@ -229,21 +238,23 @@ bool ContinentalArs548Decoder::ParseDetectionsListPacket( detection_msg.range_rate_std = detection.range_rate_std.value(); } - detection_list_callback_(std::move(msg_ptr)); + if (detection_list_callback_) { + detection_list_callback_(std::move(msg_ptr)); + } return true; } bool ContinentalArs548Decoder::ParseObjectsListPacket( - std::unique_ptr packet_msg) + const nebula_msgs::msg::NebulaPacket & packet_msg) { auto msg_ptr = std::make_unique(); auto & msg = *msg_ptr; ObjectListPacket object_list; - assert(sizeof(ObjectListPacket) == packet_msg->data.size()); + assert(sizeof(ObjectListPacket) == packet_msg.data.size()); - std::memcpy(&object_list, packet_msg->data.data(), sizeof(object_list)); + std::memcpy(&object_list, packet_msg.data.data(), sizeof(object_list)); msg.header.frame_id = config_ptr_->object_frame; @@ -251,7 +262,7 @@ bool ContinentalArs548Decoder::ParseObjectsListPacket( msg.header.stamp.nanosec = object_list.stamp.timestamp_nanoseconds.value(); msg.header.stamp.sec = object_list.stamp.timestamp_seconds.value(); } else { - msg.header.stamp = packet_msg->stamp; + msg.header.stamp = packet_msg.stamp; } msg.stamp_sync_status = object_list.stamp.timestamp_sync_status; @@ -381,16 +392,18 @@ bool ContinentalArs548Decoder::ParseObjectsListPacket( object_msg.shape_width_edge_mean = object.shape_width_edge_mean.value(); } - object_list_callback_(std::move(msg_ptr)); + if (object_list_callback_) { + object_list_callback_(std::move(msg_ptr)); + } return true; } bool ContinentalArs548Decoder::ParseSensorStatusPacket( - std::unique_ptr packet_msg) + const nebula_msgs::msg::NebulaPacket & packet_msg) { SensorStatusPacket sensor_status_packet; - std::memcpy(&sensor_status_packet, packet_msg->data.data(), sizeof(SensorStatusPacket)); + std::memcpy(&sensor_status_packet, packet_msg.data.data(), sizeof(SensorStatusPacket)); radar_status_.timestamp_nanoseconds = sensor_status_packet.stamp.timestamp_nanoseconds.value(); radar_status_.timestamp_seconds = sensor_status_packet.stamp.timestamp_seconds.value(); @@ -624,14 +637,6 @@ bool ContinentalArs548Decoder::ParseSensorStatusPacket( sensor_status_callback_(radar_status_); } - if (nebula_packets_callback_) { - auto packets_msg = std::make_unique(); - packets_msg->packets.emplace_back(std::move(*packet_msg)); - packets_msg->header.stamp = packet_msg->stamp; - packets_msg->header.frame_id = sensor_configuration_->frame_id; - nebula_packets_callback_(std::move(packets_msg)); - } - return true; } diff --git a/nebula_hw_interfaces/package.xml b/nebula_hw_interfaces/package.xml index b526108c9..7e929405b 100644 --- a/nebula_hw_interfaces/package.xml +++ b/nebula_hw_interfaces/package.xml @@ -19,6 +19,7 @@ pandar_msgs rclcpp robosense_msgs + ros2_socketcan sensor_msgs velodyne_msgs 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 0a32ad185..47dd10f2c 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 @@ -143,7 +143,7 @@ Status ContinentalArs548HwInterface::SetSensorMounting( return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -182,7 +182,7 @@ Status ContinentalArs548HwInterface::SetVehicleParameters( return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -226,7 +226,7 @@ Status ContinentalArs548HwInterface::SetRadarParameters( return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -267,7 +267,7 @@ Status ContinentalArs548HwInterface::SetSensorIPAddress(const std::string & sens return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -298,7 +298,7 @@ Status ContinentalArs548HwInterface::SetAccelerationLateralCog(float lateral_acc return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -331,7 +331,7 @@ Status ContinentalArs548HwInterface::SetAccelerationLongitudinalCog(float longit return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -362,7 +362,7 @@ Status ContinentalArs548HwInterface::SetCharacteristicSpeed(float characteristic return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -398,7 +398,7 @@ Status ContinentalArs548HwInterface::SetDrivingDirection(int direction) return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -430,7 +430,7 @@ Status ContinentalArs548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -461,7 +461,7 @@ Status ContinentalArs548HwInterface::SetVelocityVehicle(float velocity_kmh) return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -492,7 +492,7 @@ Status ContinentalArs548HwInterface::SetYawRate(float yaw_rate) return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } diff --git a/nebula_messages/continental_msgs/CMakeLists.txt b/nebula_messages/continental_msgs/CMakeLists.txt index c7b7978f6..724189b70 100644 --- a/nebula_messages/continental_msgs/CMakeLists.txt +++ b/nebula_messages/continental_msgs/CMakeLists.txt @@ -19,6 +19,10 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/ContinentalArs548DetectionList.msg" "msg/ContinentalArs548Object.msg" "msg/ContinentalArs548ObjectList.msg" + "msg/ContinentalSrr520Detection.msg" + "msg/ContinentalSrr520DetectionList.msg" + "msg/ContinentalSrr520Object.msg" + "msg/ContinentalSrr520ObjectList.msg" DEPENDENCIES std_msgs geometry_msgs diff --git a/nebula_messages/continental_msgs/msg/ContinentalSrr520Detection.msg b/nebula_messages/continental_msgs/msg/ContinentalSrr520Detection.msg new file mode 100644 index 000000000..9536ffa12 --- /dev/null +++ b/nebula_messages/continental_msgs/msg/ContinentalSrr520Detection.msg @@ -0,0 +1,11 @@ +float32 range +float32 azimuth_angle +float32 range_rate +float32 rcs +uint8 pdh00 +uint8 pdh01 +uint8 pdh02 +uint8 pdh03 +uint8 pdh04 +uint8 pdh05 +float32 snr diff --git a/nebula_messages/continental_msgs/msg/ContinentalSrr520DetectionList.msg b/nebula_messages/continental_msgs/msg/ContinentalSrr520DetectionList.msg new file mode 100644 index 000000000..e2c159901 --- /dev/null +++ b/nebula_messages/continental_msgs/msg/ContinentalSrr520DetectionList.msg @@ -0,0 +1,10 @@ +std_msgs/Header header +uint32 internal_time_stamp_usec +uint8 global_time_stamp_sync_status +uint8 signal_status +uint8 sequence_counter +uint32 cycle_counter +float32 vambig # cSpell:ignore vambig +float32 max_range + +ContinentalSrr520Detection[] detections diff --git a/nebula_messages/continental_msgs/msg/ContinentalSrr520Object.msg b/nebula_messages/continental_msgs/msg/ContinentalSrr520Object.msg new file mode 100644 index 000000000..a12787fa9 --- /dev/null +++ b/nebula_messages/continental_msgs/msg/ContinentalSrr520Object.msg @@ -0,0 +1,21 @@ +uint8 object_id +float32 dist_x +float32 dist_y +float32 v_abs_x +float32 v_abs_y +float32 a_abs_x +float32 a_abs_y +float32 dist_x_std +float32 dist_y_std +float32 v_abs_x_std +float32 v_abs_y_std +float32 a_abs_x_std +float32 a_abs_y_std +float32 box_length +float32 box_width +float32 orientation +float32 rcs +float32 score +uint16 life_cycles +uint8 box_valid +uint8 object_status diff --git a/nebula_messages/continental_msgs/msg/ContinentalSrr520ObjectList.msg b/nebula_messages/continental_msgs/msg/ContinentalSrr520ObjectList.msg new file mode 100644 index 000000000..f873eb05a --- /dev/null +++ b/nebula_messages/continental_msgs/msg/ContinentalSrr520ObjectList.msg @@ -0,0 +1,12 @@ +std_msgs/Header header + +uint32 internal_time_stamp_usec +uint8 global_time_stamp_sync_status +uint8 signal_status +uint8 sequence_counter +uint32 cycle_counter +float32 ego_vx +float32 ego_yaw_rate +uint8 motion_type + +ContinentalSrr520Object[] objects diff --git a/nebula_messages/continental_srvs/CMakeLists.txt b/nebula_messages/continental_srvs/CMakeLists.txt index ffa839825..b45d309f2 100644 --- a/nebula_messages/continental_srvs/CMakeLists.txt +++ b/nebula_messages/continental_srvs/CMakeLists.txt @@ -19,6 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/ContinentalArs548SetVehicleParameters.srv" "srv/ContinentalArs548SetRadarParameters.srv" "srv/ContinentalArs548SetNetworkConfiguration.srv" + "srv/ContinentalSrr520SetRadarParameters.srv" DEPENDENCIES std_msgs ) diff --git a/nebula_messages/continental_srvs/srv/ContinentalSrr520SetRadarParameters.srv b/nebula_messages/continental_srvs/srv/ContinentalSrr520SetRadarParameters.srv new file mode 100644 index 000000000..4d0d3b297 --- /dev/null +++ b/nebula_messages/continental_srvs/srv/ContinentalSrr520SetRadarParameters.srv @@ -0,0 +1,15 @@ +bool autoconfigure_extrinsics 1 +float32 longitudinal +float32 lateral +float32 vertical +float32 yaw +float32 pitch +float32 vehicle_wheelbase -1.0 + +uint8 sensor_id # The new sensor id +float32 cover_damping # Cover damping in dB +bool plug_bottom # Whether the plug is in the bottom +bool reset_sensor_configuration +--- +bool success +string message # status messages diff --git a/nebula_ros/config/continental/SRR520.yaml b/nebula_ros/config/continental/SRR520.yaml new file mode 100644 index 000000000..644d424f7 --- /dev/null +++ b/nebula_ros/config/continental/SRR520.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + sensor_model: "SRR520" + frame_id: "continental" + base_frame: "base_link" + interface: "can4" + receiver_timeout_sec: 0.03 + sender_timeout_sec: 0.01 + filters: "0:0" + use_bus_time: false + new_sensor_id: 0 + new_plug_bottom: true + new_cover_damping: 0.0 + reset_sensor_configuration: false + configuration_vehicle_wheelbase: 2.79