chore: spell fixes

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>

.cspell.json

@@ -27,6 +27,7 @@
     "struct",
     "structs",
     "UDP_SEQ",
+    "usec",
     "vccint",
     "Vccint",
     "XT",

nebula_common/include/nebula_common/continental/continental_srr520.hpp

@@ -219,7 +219,6 @@ constexpr int HRR_CRC_ID = 1;
 constexpr int OBJECT_CRC_ID = 2;
 constexpr int TIME_DOMAIN_ID = 0;
 
-//
 constexpr int RDI_NEAR_HEADER_PACKET_SIZE = 32;
 constexpr int RDI_NEAR_ELEMENT_PACKET_SIZE = 64;
 constexpr int RDI_HRR_HEADER_PACKET_SIZE = 32;
@@ -241,6 +240,10 @@ constexpr int FRAGMENTS_PER_OBJECT_PACKET = 2;
 constexpr int DETECTION_FRAGMENT_SIZE = 6;
 constexpr int OBJECT_FRAGMENT_SIZE = 31;
 
+constexpr int MAX_RDI_NEAR_DETECTIONS = RDI_NEAR_PACKET_NUM * FRAGMENTS_PER_DETECTION_PACKET;
+constexpr int MAX_RDI_HRR_DETECTIONS = RDI_HRR_PACKET_NUM * FRAGMENTS_PER_DETECTION_PACKET;
+constexpr int MAX_OBJECTS = OBJECT_PACKET_NUM * FRAGMENTS_PER_OBJECT_PACKET;
+
 #pragma pack(push, 1)
 
 struct StatusPacket
@@ -287,7 +290,7 @@ struct ScanHeaderPacket
   uint8_t u_signal_status;                    // 13
   uint8_t u_sequence_counter;                 // 14
   big_uint32_buf_t u_cycle_counter;           // 15
-  big_uint16_buf_t u_vambig;                  // 19
+  big_uint16_buf_t u_vambig;                  // 19. cSpell:ignore vambig
   big_uint16_buf_t u_max_range;               // 21
   big_uint16_buf_t u_number_of_detections;    // 23
   uint8_t reserved[7];                        // 25

nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp

@@ -43,7 +43,7 @@ class ContinentalSRR520Decoder : public ContinentalPacketsDecoder
   explicit ContinentalSRR520Decoder(
     const std::shared_ptr<ContinentalSRR520SensorConfiguration> & sensor_configuration);
 
-  /// @brief Function for psrring NebulaPackets
+  /// @brief Function for parsing NebulaPackets
   /// @param nebula_packets
   /// @return Resulting flag
   bool ProcessPackets(const nebula_msgs::msg::NebulaPackets & nebula_packets) override;

nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp

@@ -378,7 +378,7 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket(
 }
 
 bool ContinentalARS548Decoder::ParseSensorStatusPacket(
-  const std::vector<uint8_t> & data, const std_msgs::msg::Header & header)
+  const std::vector<uint8_t> & data, [[maybe_unused]] const std_msgs::msg::Header & header)
 {
   SensorStatusPacket sensor_status_packet;
   std::memcpy(&sensor_status_packet, data.data(), sizeof(SensorStatusPacket));

nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp

@@ -120,15 +120,15 @@ bool ContinentalSRR520Decoder::ParseDetectionsListPacket(
 
   if (header_packet.u_global_time_stamp_sync_status == 1) {
     scan_msg->header.stamp.sec = header_packet.u_global_time_stamp_sec.value();
-    scan_msg->header.stamp.sec = header_packet.u_global_time_stamp_nsec.value();
+    scan_msg->header.stamp.nanosec = header_packet.u_global_time_stamp_nsec.value();
   }
 
   scan_msg->internal_time_stamp_usec = header_packet.u_time_stamp.value();
   scan_msg->global_time_stamp_sync_status = header_packet.u_global_time_stamp_sync_status;
   scan_msg->signal_status = header_packet.u_signal_status;
   scan_msg->sequence_counter = header_packet.u_sequence_counter;
   scan_msg->cycle_counter = header_packet.u_cycle_counter.value();
-  scan_msg->vambig = 0.003051851f * header_packet.u_vambig.value() - 100.f;
+  scan_msg->vambig = 0.003051851f * header_packet.u_vambig.value() - 100.f;  // cSpell:ignore vambig
   scan_msg->max_range = 0.1f * header_packet.u_max_range.value();
 
   int parsed_detections = 0;
@@ -218,7 +218,7 @@ bool ContinentalSRR520Decoder::ParseObjectsListPacket(
 
   if (header_packet.u_global_time_stamp_sync_status == 1) {
     objects_msg->header.stamp.sec = header_packet.u_global_time_stamp_sec.value();
-    objects_msg->header.stamp.sec = header_packet.u_global_time_stamp_nsec.value();
+    objects_msg->header.stamp.nanosec = header_packet.u_global_time_stamp_nsec.value();
   }
 
   objects_msg->internal_time_stamp_usec = header_packet.u_time_stamp.value();
@@ -354,7 +354,7 @@ bool ContinentalSRR520Decoder::ParseStatusPacket(
 
   auto & diagnostic_status = diagnostic_array_msg_ptr->status.front();
   auto & diagnostic_values = diagnostic_status.values;
-  diagnostic_values.reserve(33);  // add number later
+  diagnostic_values.reserve(33);
   diagnostic_status.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
 
   diagnostic_status.message = "Sensor diagnostics for the SRR520";
@@ -446,13 +446,6 @@ bool ContinentalSRR520Decoder::ParseStatusPacket(
                               ? diagnostic_msgs::msg::DiagnosticStatus::ERROR
                               : diagnostic_status.level;
 
-  key_value.key = "sensor_off_temp";
-  key_value.value = std::to_string(status_packet.u_sensor_off_temp);
-  diagnostic_values.push_back(key_value);
-  diagnostic_status.level = status_packet.u_sensor_off_temp != 0
-                              ? diagnostic_msgs::msg::DiagnosticStatus::ERROR
-                              : diagnostic_status.level;
-
   key_value.key = "long_vel_invalid";
   key_value.value = std::to_string(status_packet.u_dynamics_invalid0 & 0b00000011);
   diagnostic_values.push_back(key_value);

nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp

@@ -81,6 +81,7 @@ class ContinentalSRR520HwInterface : NebulaHwInterfaceBase
 
   uint8_t sync_counter_{0};
   bool sync_fup_sent_{true};
+  builtin_interfaces::msg::Time last_sync_stamp_;
 
   std::shared_ptr<rclcpp::Logger> parent_node_logger;
 

nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp

@@ -124,10 +124,6 @@ void ContinentalSRR520HwInterface::ReceiveLoop()
 
     buffer.resize(receive_id.length());
 
-    // std::cout << "CAN_ID=" << receive_id.identifier() << " | extended=" <<
-    // receive_id.is_extended() << " | is_error=" << (receive_id.frame_type() ==
-    // ::drivers::socketcan::FrameType::ERROR) << " | len=" << receive_id.length() << std::endl <<
-    // std::flush;
     int64_t stamp = use_bus_time
                       ? static_cast<int64_t>(receive_id.get_bus_time() * 1000U)
                       : static_cast<uint64_t>(std::chrono::duration_cast<std::chrono::nanoseconds>(
@@ -354,7 +350,7 @@ void ContinentalSRR520HwInterface::ProcessCRCListPacket(
   if (crc_id == NEAR_CRC_ID) {
     ProcessNearCRCListPacket(buffer, stamp);
   } else if (crc_id == HRR_CRC_ID) {
-    ProcessHRRCRCListPacket(buffer, stamp);
+    ProcessHRRCRCListPacket(buffer, stamp);  // cspell: ignore HRRCRC
   } else if (crc_id == OBJECT_CRC_ID) {
     ProcessObjectCRCListPacket(buffer, stamp);
   } else {
@@ -376,8 +372,6 @@ void ContinentalSRR520HwInterface::ProcessNearCRCListPacket(
   uint16_t computed_crc =
     crc16_packets(rdi_near_packets_ptr_->packets.begin() + 1, rdi_near_packets_ptr_->packets.end());
 
-  // uint8_t current_seq = buffer[3];
-
   if (transmitted_crc != computed_crc) {
     PrintError(
       "RDI Near: Transmitted CRC list does not coincide with the computed one. Ignoring packet");
@@ -403,8 +397,6 @@ void ContinentalSRR520HwInterface::ProcessHRRCRCListPacket(
   uint16_t computed_crc =
     crc16_packets(rdi_hrr_packets_ptr_->packets.begin() + 1, rdi_hrr_packets_ptr_->packets.end());
 
-  // uint8_t current_seq = buffer[3];
-
   if (transmitted_crc != computed_crc) {
     PrintError(
       "RDI HRR: Transmitted CRC list does not coincide with the computed one. Ignoring packet");
@@ -465,46 +457,26 @@ void ContinentalSRR520HwInterface::ProcessSensorStatusPacket(
 }
 
 void ContinentalSRR520HwInterface::ProcessSyncFupPacket(
-  const std::vector<uint8_t> & buffer, [[maybe_unused]] const uint64_t stamp)
+  [[maybe_unused]] const std::vector<uint8_t> & buffer, [[maybe_unused]] const uint64_t stamp)
 {
   if (!can_sender_) {
     PrintError("Can sender is invalid so can not do follow up");
   }
 
-  bool is_sync = buffer[0] == 0x20;
-  uint8_t rx_sync_counter = buffer[2] & 0x0F;
-  uint8_t rx_domain_id = buffer[2] >> 4;
-
-  uint32_t sec = stamp / 1'000'000'000;
-  uint32_t nanosec = stamp % 1'000'000'000;
-
   if (!sensor_configuration_->sync_use_bus_time || sync_fup_sent_) {
     return;
   }
 
-  // In this case, we will attempt to do the full sync scheme
-
-  auto now = std::chrono::system_clock::now();
-  auto now_secs = std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch()).count();
-  auto now_nanosecs =
-    std::chrono::duration_cast<std::chrono::nanoseconds>(now.time_since_epoch()).count();
-
-  builtin_interfaces::msg::Time fup_stamp;
-  fup_stamp.sec = static_cast<int>(now_secs);
-  fup_stamp.nanosec = static_cast<std::uint32_t>(now_nanosecs % 1'000'000'000);
-
-  auto t0s = fup_stamp;
+  auto t0s = last_sync_stamp_;
   t0s.nanosec = 0;
-  const auto t1r = fup_stamp;
+  const auto t1r = stamp;
 
   builtin_interfaces::msg::Time t4r =
     rclcpp::Time(rclcpp::Time() + (rclcpp::Time(t1r) - rclcpp::Time(t0s)));
   uint8_t t4r_seconds = static_cast<uint8_t>(t4r.sec);
   uint32_t t4r_nanoseconds = t4r.nanosec;
-
   std::array<uint8_t, 8> data;
   data[0] = 0x28;  // mode 0x18 is without CRC
-  data[1] = 0;     // CRC. Not this time
   data[2] = (((static_cast<uint16_t>(TIME_DOMAIN_ID) << 4) & 0xF0)) |
             (sync_counter_ & 0x0F);  // Domain and counter
   data[3] = t4r_seconds & 0x3;       // SGW and OVS
@@ -541,10 +513,10 @@ void ContinentalSRR520HwInterface::SensorSync()
   builtin_interfaces::msg::Time stamp;
   stamp.sec = static_cast<int>(now_secs);
   stamp.nanosec = static_cast<std::uint32_t>(now_nanosecs % 1'000'000'000);
+  last_sync_stamp_ = stamp;
 
   std::array<uint8_t, 8> data;
   data[0] = 0x20;  // mode 0x10 is without CRC
-  data[1] = 0;     // CRC. Not this time
   data[2] = (((static_cast<uint16_t>(TIME_DOMAIN_ID) << 4) & 0xF0)) |
             (sync_counter_ & 0x0F);  // Domain and counter
   data[3] = 0;                       // use data
@@ -565,7 +537,6 @@ void ContinentalSRR520HwInterface::SensorSync()
   }
 
   data[0] = 0x28;  // mode 0x18 is without CRC
-  data[1] = 0;     // CRC. Not this time
   data[2] = (((static_cast<uint16_t>(TIME_DOMAIN_ID) << 4) & 0xF0)) |
             (sync_counter_ & 0x0F);  // Domain and counter
   data[3] = 0;                       // SGW and OVS
@@ -659,31 +630,8 @@ Status ContinentalSRR520HwInterface::ConfigureSensor(
   const uint16_t u_yaw_angle = static_cast<uint16_t>((yaw_autosar + 3.14159f) / 9.5877e-05);
   const uint16_t u_cover_damping = static_cast<uint16_t>((cover_damping + 32.767f) / 0.001f);
 
-  std::cout << "\t\t\tu_LongPos: " << std::hex << static_cast<uint16_t>(u_long_pos) << " | "
-            << std::bitset<16>(u_long_pos) << std::endl;
-  std::cout << "\t\t\tu_LatPos: " << std::hex << static_cast<uint16_t>(u_lat_pos) << " | "
-            << std::bitset<16>(u_lat_pos) << std::endl;
-  std::cout << "\t\t\tu_VertPos: " << std::hex << static_cast<uint16_t>(u_vert_pos) << " | "
-            << std::bitset<16>(u_vert_pos) << std::endl;
-  std::cout << "\t\t\tu_LongPosCoG: " << std::hex << static_cast<uint16_t>(u_long_pos_cog) << " | "
-            << std::bitset<16>(u_long_pos_cog) << std::endl;
-  std::cout << "\t\t\tu_Wheelbase: " << std::hex << static_cast<uint16_t>(u_wheelbase) << " | "
-            << std::bitset<16>(u_wheelbase) << std::endl;
-  std::cout << "\t\t\tu_YawAngle: " << std::hex << static_cast<uint16_t>(u_yaw_angle) << " | "
-            << std::bitset<16>(u_yaw_angle) << std::endl;
-  std::cout << "\t\t\tu_CoverDamping: " << std::hex << static_cast<uint16_t>(u_cover_damping)
-            << " | " << std::bitset<16>(u_cover_damping) << std::endl;
-
-  std::cout << "\t\t\tu_LatPos1: " << std::hex << static_cast<uint16_t>(u_lat_pos) << " | "
-            << std::bitset<16>(u_lat_pos) << std::endl;
-  std::cout << "\t\t\tu_LatPos2: " << std::hex << static_cast<uint16_t>(u_lat_pos & 0xff00) << " | "
-            << std::bitset<16>(u_lat_pos & 0xff00) << std::endl;
-  std::cout << "\t\t\tu_LatPos3: " << std::hex << static_cast<uint16_t>((u_lat_pos & 0xff00) >> 8)
-            << " | " << std::bitset<8>((u_lat_pos & 0xff00) >> 8) << std::endl;
-
   std::array<uint8_t, 16> data;
-  data[0] = sensor_id;  // 0 actually makes it 1. When there are multiple radars in the same
-                        // vehicle, this actually is needed
+  data[0] = sensor_id;
   data[1] = static_cast<uint8_t>((u_long_pos & 0xff00) >> 8);
   data[2] = static_cast<uint8_t>((u_long_pos & 0x00ff));
 
@@ -707,7 +655,7 @@ Status ContinentalSRR520HwInterface::ConfigureSensor(
 
   uint8_t plug_value = plug_bottom ? 0x00 : 0x01;
   uint8_t reset_value = reset ? 0x80 : 0x00;
-  data[15] = plug_value | reset_value;  // PLUG orientation. default is 0, make it 1 for testing
+  data[15] = plug_value | reset_value;
 
   if (SendFrame(data, SENSOR_CONFIG_CAN_MESSAGE_ID)) {
     return Status::OK;

nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp

@@ -56,7 +56,7 @@ class NebulaHwInterfaceWrapperBase
   virtual Status InitializeHwInterface(
     const drivers::SensorConfigurationBase & sensor_configuration) = 0;
   //  void SendDataPacket(const std::vector<uint8_t> &buffer);        // Ideally this will be
-  //  implemented as specific funtions, GetFanStatus, GetEchoMode
+  //  implemented as specific functions, GetFanStatus, GetEchoMode
 
   /// @brief Enable sensor setup during initialization and set_parameters_callback
   bool setup_sensor;

nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_ros_wrapper.hpp

@@ -88,7 +88,6 @@ class ContinentalSRR520HwInterfaceRosWrapper final : public rclcpp::Node,
   std::shared_ptr<ExactTimeSync> sync_ptr_;
 
   rclcpp::TimerBase::SharedPtr sync_timer_;
-  rclcpp::TimerBase::SharedPtr vehicle_dynamics_timer_;
 
   bool standstill_{true};
 
@@ -117,8 +116,8 @@ class ContinentalSRR520HwInterfaceRosWrapper final : public rclcpp::Node,
     const std::shared_ptr<std_srvs::srv::Empty::Request> request,
     const std::shared_ptr<std_srvs::srv::Empty::Response> response);
 
+  /// @brief Method periodically called to initiate the sensor synchronization mechanism
   void syncTimerCallback();
-  void vehicleDynamicsTimerCallback();
 
 public:
   explicit ContinentalSRR520HwInterfaceRosWrapper(const rclcpp::NodeOptions & options);

nebula_ros/src/continental/continental_srr520_decoder_ros_wrapper.cpp

@@ -575,9 +575,13 @@ visualization_msgs::msg::MarkerArray ContinentalSRR520DriverRosWrapper::ConvertT
   }};
 
   std::unordered_set<int> current_ids;
+  std::size_t null_object_id_counter = 0;
 
-  radar_msgs::msg::RadarTrack track_msg;
   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;
@@ -589,7 +593,9 @@ visualization_msgs::msg::MarkerArray ContinentalSRR520DriverRosWrapper::ConvertT
     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.id = object.object_id == 0
+                      ? drivers::continental_srr520::MAX_OBJECTS + null_object_id_counter++
+                      : 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);

nebula_ros/src/continental/continental_srr520_hw_interface_ros_wrapper.cpp

@@ -84,10 +84,6 @@ Status ContinentalSRR520HwInterfaceRosWrapper::StreamStart()
     sync_timer_ = rclcpp::create_timer(
       this, get_clock(), 100ms,
       std::bind(&ContinentalSRR520HwInterfaceRosWrapper::syncTimerCallback, this));
-
-    vehicle_dynamics_timer_ = rclcpp::create_timer(
-      this, get_clock(), 100ms,
-      std::bind(&ContinentalSRR520HwInterfaceRosWrapper::vehicleDynamicsTimerCallback, this));
   }
 
   return interface_status_;
@@ -378,11 +374,6 @@ void ContinentalSRR520HwInterfaceRosWrapper::syncTimerCallback()
   hw_interface_.SensorSync();
 }
 
-void ContinentalSRR520HwInterfaceRosWrapper::vehicleDynamicsTimerCallback()
-{
-  hw_interface_.SetVehicleDynamics(0.0, 0.0, 0.0, 0.0, true);
-}
-
 std::vector<rcl_interfaces::msg::SetParametersResult>
 ContinentalSRR520HwInterfaceRosWrapper::updateParameters()
 {