diff --git a/modules/drivers/gnss/util/macros.h b/modules/common/util/macros.h similarity index 78% rename from modules/drivers/gnss/util/macros.h rename to modules/common/util/macros.h index d348621eb7f..e3454f760a6 100644 --- a/modules/drivers/gnss/util/macros.h +++ b/modules/common/util/macros.h @@ -21,18 +21,6 @@ #include -// Put this in the declarations for a class to be uncopyable. -#define DISABLE_COPY(TypeName) TypeName(const TypeName &) = delete - -// Put this in the declarations for a class to be unassignable. -#define DISABLE_ASSIGN(TypeName) void operator=(const TypeName &) = delete - -// A macro to disallow the copy constructor and operator= functions. -// This should be used in the private: declarations for a class. -#define DISABLE_COPY_AND_ASSIGN(TypeName) \ - DISABLE_COPY(TypeName); \ - DISABLE_ASSIGN(TypeName) - // A macro to disallow all the implicit constructors, namely the // default constructor, copy constructor and operator= functions. // diff --git a/modules/common/util/util.h b/modules/common/util/util.h index ad194426a71..f847a9c1158 100644 --- a/modules/common/util/util.h +++ b/modules/common/util/util.h @@ -29,12 +29,13 @@ #include #include +#include "modules/common_msgs/basic_msgs/geometry.pb.h" +#include "modules/common_msgs/basic_msgs/pnc_point.pb.h" + #include "cyber/common/log.h" #include "cyber/common/types.h" #include "modules/common/configs/config_gflags.h" #include "modules/common/math/vec2d.h" -#include "modules/common_msgs/basic_msgs/geometry.pb.h" -#include "modules/common_msgs/basic_msgs/pnc_point.pb.h" /** * @namespace apollo::common::util @@ -128,6 +129,7 @@ IsFloatEqual(T x, T y, int ulp = 2) { // unless the result is subnormal || std::fabs(x - y) < std::numeric_limits::min(); } + } // namespace util } // namespace common } // namespace apollo diff --git a/modules/drivers/gnss/conf/gnss_conf.pb.txt b/modules/drivers/gnss/conf/gnss_conf.pb.txt index 410dcd05481..55fa4a22237 100644 --- a/modules/drivers/gnss/conf/gnss_conf.pb.txt +++ b/modules/drivers/gnss/conf/gnss_conf.pb.txt @@ -1,7 +1,7 @@ data { format: NOVATEL_BINARY serial { - device: "/dev/novatel0" + device: "/dev/ttyACM0" baud_rate: 115200 } } @@ -22,7 +22,7 @@ data { rtk_to { format: NOVATEL_BINARY serial { - device: "/dev/novatel1" + device: "/dev/ttyACM1" baud_rate: 115200 } } @@ -30,7 +30,7 @@ rtk_to { command { format: NOVATEL_BINARY serial { - device: "/dev/novatel2" + device: "/dev/ttyACM2" baud_rate: 115200 } } @@ -41,7 +41,7 @@ proj4_text: "+proj=utm +zone=10 +ellps=WGS84 +towgs84=0,0,0,0,0,0,0 +units=m +no tf { frame_id: "world" - child_frame_id: "novatel" + child_frame_id: "imu" enable: true } diff --git a/modules/drivers/gnss/parser/BUILD b/modules/drivers/gnss/parser/BUILD index 38f7e7db58b..5f4070758d9 100644 --- a/modules/drivers/gnss/parser/BUILD +++ b/modules/drivers/gnss/parser/BUILD @@ -4,81 +4,58 @@ load("//tools:cpplint.bzl", "cpplint") package(default_visibility = ["//visibility:public"]) cc_library( - name = "gnss_parser", + name = "parser", + hdrs = [ + "parser.h", + ], deps = [ - ":data_parser", - ":novatel_parser", - ":rtcm_parsers", + "//cyber", + "//modules/drivers/gnss/proto:config_cc_proto", + ":data_buffer", ], ) cc_library( name = "data_parser", - srcs = ["data_parser.cc"], + srcs = [ + "data_parser.cc", + ], hdrs = [ "data_parser.h", - "parser.h", ], - copts = ["-Ithird_party/rtklib"], deps = [ - ":novatel_parser", + ":parser", "//cyber", "//modules/common/adapters:adapter_gflags", "//modules/common/util:util_tool", + "//modules/drivers/gnss/util:gnss_util", "//modules/common_msgs/sensor_msgs:gnss_best_pose_cc_proto", "//modules/common_msgs/sensor_msgs:gnss_cc_proto", "//modules/drivers/gnss/proto:gnss_status_cc_proto", - "//modules/drivers/gnss/util:gnss_util", "//modules/common_msgs/localization_msgs:gps_cc_proto", "//modules/common_msgs/localization_msgs:imu_cc_proto", + "//modules/common_msgs/localization_msgs:ins_cc_proto", + "//modules/common_msgs/localization_msgs:heading_cc_proto", "//modules/transform:transform_broadcaster", "@eigen", "@proj", ], ) -cc_library( - name = "novatel_parser", - srcs = ["novatel_parser.cc"], - hdrs = [ - "novatel_messages.h", - "parser.h", - "rtcm_decode.h", - ], - copts = ["-Ithird_party/rtklib"], - deps = [ - "//cyber", - "//modules/common/adapters:adapter_gflags", - "//modules/common_msgs/basic_msgs:error_code_cc_proto", - "//modules/common_msgs/basic_msgs:geometry_cc_proto", - "//modules/common_msgs/basic_msgs:header_cc_proto", - "//modules/drivers/gnss/proto:config_cc_proto", - "//modules/common_msgs/sensor_msgs:gnss_best_pose_cc_proto", - "//modules/common_msgs/sensor_msgs:gnss_cc_proto", - "//modules/common_msgs/sensor_msgs:gnss_raw_observation_cc_proto", - "//modules/common_msgs/sensor_msgs:heading_cc_proto", - "//modules/common_msgs/sensor_msgs:imu_cc_proto", - "//modules/common_msgs/sensor_msgs:ins_cc_proto", - "//modules/drivers/gnss/util:gnss_util", - "//third_party/rtklib", - ], -) - cc_library( name = "rtcm_parsers", srcs = [ - "rtcm3_parser.cc", "rtcm_parser.cc", ], hdrs = [ "parser.h", - "rtcm3_parser.h", "rtcm_decode.h", "rtcm_parser.h", ], copts = ["-Ithird_party/rtklib"], deps = [ "//cyber", + "//modules/drivers/gnss/parser/rtcm3:rtcm3_parser", "//modules/common/adapters:adapter_gflags", "//modules/drivers/gnss/proto:config_cc_proto", "//modules/common_msgs/sensor_msgs:gnss_raw_observation_cc_proto", diff --git a/modules/drivers/gnss/parser/data_parser.cc b/modules/drivers/gnss/parser/data_parser.cc index 6c0fcdf9fb6..d8adfdaf07f 100644 --- a/modules/drivers/gnss/parser/data_parser.cc +++ b/modules/drivers/gnss/parser/data_parser.cc @@ -19,18 +19,14 @@ #include #include #include +#include #include "Eigen/Geometry" #include "boost/array.hpp" + #include "cyber/cyber.h" #include "modules/common/adapters/adapter_gflags.h" #include "modules/common/util/message_util.h" -#include "modules/common_msgs/sensor_msgs/gnss_best_pose.pb.h" -#include "modules/common_msgs/sensor_msgs/gnss_raw_observation.pb.h" -#include "modules/common_msgs/sensor_msgs/heading.pb.h" -#include "modules/common_msgs/localization_msgs/imu.pb.h" - -#include "modules/drivers/gnss/parser/parser.h" #include "modules/drivers/gnss/util/time_conversion.h" namespace apollo { @@ -45,32 +41,42 @@ using apollo::transform::TransformStamped; namespace { constexpr double DEG_TO_RAD_LOCAL = M_PI / 180.0; -const char *WGS84_TEXT = "+proj=latlong +ellps=WGS84"; +// Using EPSG:4326 for WGS84 geographic is standard. +// Alternatively, could use "+proj=latlong +ellps=WGS84 +datum=WGS84" with +// proj_create +const char *WGS84_TEXT = "EPSG:4326"; // covariance data for pose if can not get from novatel inscov topic -static const boost::array POSE_COVAR = { +// Marked constexpr as it's compile-time constant +static constexpr boost::array POSE_COVAR = { 2, 0, 0, 0, 0, 0, 0, 2, 0, 0, 0, 0, 0, 0, 2, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01}; -Parser *CreateParser(config::Config config, bool is_base_station = false) { - switch (config.data().format()) { - case config::Stream::NOVATEL_BINARY: - return Parser::CreateNovatel(config); - - default: - return nullptr; - } -} - } // namespace DataParser::DataParser(const config::Config &config, const std::shared_ptr &node) : config_(config), tf_broadcaster_(node), node_(node) { - std::string utm_target_param; + // Use modern PROJ API for initialization + // proj_create_crs_to_crs is recommended for transformations between CRS + // proj_create can be used with proj strings like config_.proj4_text() + wgs84pj_source_ = proj_create(PJ_DEFAULT_CTX, WGS84_TEXT); + if (!wgs84pj_source_) { + AFATAL << "Failed to create WGS84 PROJ object: " + << proj_context_errno_string(PJ_DEFAULT_CTX); + // Handle error appropriately, maybe throw exception or set init_flag_ to + // false initially For now, logging AFATAL as original code didn't handle + // this failure either. + } + + utm_target_ = proj_create(PJ_DEFAULT_CTX, config_.proj4_text().c_str()); + if (!utm_target_) { + AFATAL << "Failed to create UTM PROJ object from config '" + << config_.proj4_text() + << "': " << proj_context_errno_string(PJ_DEFAULT_CTX); + // Handle error appropriately + } - wgs84pj_source_ = pj_init_plus(WGS84_TEXT); - utm_target_ = pj_init_plus(config_.proj4_text().c_str()); gnss_status_.set_solution_status(0); gnss_status_.set_num_sats(0); gnss_status_.set_position_type(0); @@ -78,11 +84,24 @@ DataParser::DataParser(const config::Config &config, ins_status_.set_type(InsStatus::INVALID); } +// Destructor to clean up PROJ resources +DataParser::~DataParser() { + if (wgs84pj_source_) { + proj_destroy(wgs84pj_source_); + wgs84pj_source_ = nullptr; // Good practice + } + if (utm_target_) { + proj_destroy(utm_target_); + utm_target_ = nullptr; // Good practice + } +} + bool DataParser::Init() { - ins_status_.mutable_header()->set_timestamp_sec( - cyber::Time::Now().ToSecond()); - gnss_status_.mutable_header()->set_timestamp_sec( - cyber::Time::Now().ToSecond()); + // Check if PROJ initialization failed in constructor + if (!wgs84pj_source_ || !utm_target_) { + AFATAL << "PROJ objects not initialized. Cannot proceed."; + return false; + } gnssstatus_writer_ = node_->CreateWriter(FLAGS_gnss_status_topic); insstatus_writer_ = node_->CreateWriter(FLAGS_ins_status_topic); @@ -98,17 +117,15 @@ bool DataParser::Init() { rawimu_writer_ = node_->CreateWriter(FLAGS_raw_imu_topic); gps_writer_ = node_->CreateWriter(FLAGS_gps_topic); + // Fill and publish initial status common::util::FillHeader("gnss", &ins_status_); insstatus_writer_->Write(ins_status_); common::util::FillHeader("gnss", &gnss_status_); gnssstatus_writer_->Write(gnss_status_); AINFO << "Creating data parser of format: " << config_.data().format(); - data_parser_.reset(CreateParser(config_, false)); - if (!data_parser_) { - AFATAL << "Failed to create data parser."; - return false; - } + gnss_parser_.reset(Parser::Create(config_)); + CHECK_NOTNULL(gnss_parser_); init_flag_ = true; return true; @@ -116,26 +133,43 @@ bool DataParser::Init() { void DataParser::ParseRawData(const std::string &msg) { if (!init_flag_) { - AERROR << "Data parser not init."; + AERROR << "Data parser not init or PROJ initialization failed."; + return; + } + if (!gnss_parser_) { + AERROR << "GNSS parser is not initialized."; return; } - data_parser_->Update(msg); - Parser::MessageType type; - MessagePtr msg_ptr; - - while (cyber::OK()) { - type = data_parser_->GetMessage(&msg_ptr); - if (type == Parser::MessageType::NONE) { - break; + gnss_parser_->AppendData(msg); + + auto messages = gnss_parser_->ParseAllMessages(); + + for (const auto &[msg_type, msg_variant] : messages) { + if (std::holds_alternative>(msg_variant)) { + // Store raw byte messages. Currently only GPGGA is stored. + // This is needed if raw NMEA strings are required elsewhere. + if (msg_type == Parser::MessageType::GPGGA) { + message_map_[Parser::MessageType::GPGGA] = + std::get>(msg_variant); + } else { + // Handle other raw message types if needed, or ignore + // ADEBUG << "Received unhandled raw byte message type: " << + // static_cast(msg_type); + } + } else if (std::holds_alternative(msg_variant)) { + DispatchMessage(msg_type, std::get(msg_variant)); + } else { + AERROR << "Unknown message type variant."; } - DispatchMessage(type, msg_ptr); } } void DataParser::CheckInsStatus(::apollo::drivers::gnss::Ins *ins) { static double last_notify = cyber::Time().Now().ToSecond(); - double now = cyber::Time().Now().ToSecond(); + double now = cyber::Time::Now().ToSecond(); + + // Only update and publish if status changes or if 1 second has passed if (ins_status_record_ != static_cast(ins->type()) || (now - last_notify) > 1.0) { last_notify = now; @@ -144,23 +178,23 @@ void DataParser::CheckInsStatus(::apollo::drivers::gnss::Ins *ins) { case apollo::drivers::gnss::Ins::GOOD: ins_status_.set_type(apollo::drivers::gnss::InsStatus::GOOD); break; - case apollo::drivers::gnss::Ins::CONVERGING: ins_status_.set_type(apollo::drivers::gnss::InsStatus::CONVERGING); break; - case apollo::drivers::gnss::Ins::INVALID: default: ins_status_.set_type(apollo::drivers::gnss::InsStatus::INVALID); break; } - common::util::FillHeader("gnss", &ins_status_); - insstatus_writer_->Write(ins_status_); + insstatus_writer_->Write(ins_status_); // Use const& overload } } void DataParser::CheckGnssStatus(::apollo::drivers::gnss::Gnss *gnss) { + // Update status always, publish is handled implicitly by Cyber writer if + // needed. Could add logic here to only publish on status change if desired, + // similar to CheckInsStatus gnss_status_.set_solution_status( static_cast(gnss->solution_status())); gnss_status_.set_num_sats(static_cast(gnss->num_sats())); @@ -175,164 +209,286 @@ void DataParser::CheckGnssStatus(::apollo::drivers::gnss::Gnss *gnss) { gnssstatus_writer_->Write(gnss_status_); } -void DataParser::DispatchMessage(Parser::MessageType type, MessagePtr message) { +void DataParser::DispatchMessage(Parser::MessageType type, + Parser::ProtoMessagePtr message) { + CHECK_NOTNULL(message); + switch (type) { case Parser::MessageType::GNSS: CheckGnssStatus(As<::apollo::drivers::gnss::Gnss>(message)); break; - case Parser::MessageType::BEST_GNSS_POS: PublishBestpos(message); break; - case Parser::MessageType::IMU: PublishImu(message); break; - case Parser::MessageType::INS: CheckInsStatus(As<::apollo::drivers::gnss::Ins>(message)); PublishCorrimu(message); PublishOdometry(message); break; - case Parser::MessageType::INS_STAT: PublishInsStat(message); break; - case Parser::MessageType::BDSEPHEMERIDES: case Parser::MessageType::GPSEPHEMERIDES: case Parser::MessageType::GLOEPHEMERIDES: PublishEphemeris(message); break; - case Parser::MessageType::OBSERVATION: PublishObservation(message); break; - case Parser::MessageType::HEADING: PublishHeading(message); break; - default: + // Handle unknown message types - can log or ignore + // ADEBUG << "Received unhandled Protobuf message type: " << + // static_cast(type); break; } } -void DataParser::PublishInsStat(const MessagePtr message) { - auto ins_stat = std::make_shared(*As(message)); - common::util::FillHeader("gnss", ins_stat.get()); - insstat_writer_->Write(ins_stat); +void DataParser::PublishInsStat(const Parser::ProtoMessagePtr message) { + // Get the underlying protobuf message + InsStat *ins_stat_ptr = As(message); + // Create message directly and publish + InsStat ins_stat = *ins_stat_ptr; + common::util::FillHeader("gnss", &ins_stat); + insstat_writer_->Write(ins_stat); // Use const& overload } -void DataParser::PublishBestpos(const MessagePtr message) { - auto bestpos = std::make_shared(*As(message)); - common::util::FillHeader("gnss", bestpos.get()); - gnssbestpose_writer_->Write(bestpos); +void DataParser::PublishBestpos(const Parser::ProtoMessagePtr message) { + // Get the underlying protobuf message + GnssBestPose *bestpos_ptr = As(message); + // Create message directly and publish + GnssBestPose bestpos = *bestpos_ptr; + common::util::FillHeader("gnss", &bestpos); + gnssbestpose_writer_->Write(bestpos); // Use const& overload } -void DataParser::PublishImu(const MessagePtr message) { - auto raw_imu = std::make_shared(*As(message)); - Imu *imu = As(message); - - raw_imu->mutable_linear_acceleration()->set_x( - -imu->linear_acceleration().y()); - raw_imu->mutable_linear_acceleration()->set_y(imu->linear_acceleration().x()); - raw_imu->mutable_linear_acceleration()->set_z(imu->linear_acceleration().z()); - - raw_imu->mutable_angular_velocity()->set_x(-imu->angular_velocity().y()); - raw_imu->mutable_angular_velocity()->set_y(imu->angular_velocity().x()); - raw_imu->mutable_angular_velocity()->set_z(imu->angular_velocity().z()); - - common::util::FillHeader("gnss", raw_imu.get()); - rawimu_writer_->Write(raw_imu); +void DataParser::PublishImu(const Parser::ProtoMessagePtr message) { + Imu *imu_in = As(message); + // Create a *new* Imu message and populate it after transformation + auto raw_imu_out = std::make_shared(); + + // --- Coordinate System Transformation (Example: Novatel IMU to Apollo + // Vehicle) --- Assuming sensor frame: +X forward, +Y right, +Z down Assuming + // target frame (Apollo): +X forward, +Y left, +Z up Rotation: Swap X and Y, + // negate new X (+Y in sensor -> -X in target), keep Z Angular Velocity: + // Similarly transform angular velocities + raw_imu_out->mutable_linear_acceleration()->set_x( + -imu_in->linear_acceleration() + .y()); // Sensor +Y (right) becomes Target -X (forward) + raw_imu_out->mutable_linear_acceleration()->set_y( + imu_in->linear_acceleration() + .x()); // Sensor +X (forward) becomes Target +Y (left) + raw_imu_out->mutable_linear_acceleration()->set_z( + imu_in->linear_acceleration() + .z()); // Sensor +Z (down) becomes Target +Z (up) - Check sign based + // on convention + + raw_imu_out->mutable_angular_velocity()->set_x( + -imu_in->angular_velocity() + .y()); // Sensor +Y (right) becomes Target -X (roll) + raw_imu_out->mutable_angular_velocity()->set_y( + imu_in->angular_velocity() + .x()); // Sensor +X (forward) becomes Target +Y (pitch) + raw_imu_out->mutable_angular_velocity()->set_z( + imu_in->angular_velocity() + .z()); // Sensor +Z (down) becomes Target +Z (yaw) - Check sign based + // on convention + + common::util::FillHeader("gnss", raw_imu_out.get()); + rawimu_writer_->Write(raw_imu_out); // Using shared_ptr for created message } -void DataParser::PublishOdometry(const MessagePtr message) { +void DataParser::PublishOdometry(const Parser::ProtoMessagePtr message) { Ins *ins = As(message); + // Create a *new* Gps message (used for Odometry topic in Apollo) auto gps = std::make_shared(); double unix_sec = apollo::drivers::util::gps2unix(ins->measurement_time()); gps->mutable_header()->set_timestamp_sec(unix_sec); auto *gps_msg = gps->mutable_localization(); - // 1. pose xyz - double x = ins->position().lon(); - double y = ins->position().lat(); - x *= DEG_TO_RAD_LOCAL; - y *= DEG_TO_RAD_LOCAL; + // 1. pose xyz (WGS84 to UTM transformation) + double lon = ins->position().lon(); + double lat = ins->position().lat(); + double height = ins->position().height(); - pj_transform(wgs84pj_source_, utm_target_, 1, 1, &x, &y, NULL); + if (!wgs84pj_source_ || !utm_target_) { + AERROR << "PROJ objects not initialized for transformation."; + // Cannot transform, maybe skip this message + return; + } - gps_msg->mutable_position()->set_x(x); - gps_msg->mutable_position()->set_y(y); - gps_msg->mutable_position()->set_z(ins->position().height()); + // Use modern PROJ transformation + PJ_COORD source_coord = + proj_coord(lon, lat, height, 0); // Use height, time is 0 if not needed + PJ_COORD target_coord = + proj_trans(wgs84pj_source_, utm_target_, source_coord); + + // Check for PROJ transformation errors + int err = proj_errno(utm_target_); + if (err != 0) { + AERROR << "PROJ transformation error: " << proj_errno_string(err); + // Depending on policy, could return or publish with known bad coordinates + // For now, return to avoid publishing garbage data + return; + } + + gps_msg->mutable_position()->set_x( + target_coord.xyz.x); // Easting (transformed longitude) + gps_msg->mutable_position()->set_y( + target_coord.xyz.y); // Northing (transformed latitude) + gps_msg->mutable_position()->set_z( + target_coord.xyz.z); // Keep original height - // 2. orientation Eigen::Quaterniond q = Eigen::AngleAxisd(ins->euler_angles().z() - 90 * DEG_TO_RAD_LOCAL, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(-ins->euler_angles().y(), Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(ins->euler_angles().x(), Eigen::Vector3d::UnitY()); + // Comment: This quaternion composition applies rotations around Z, X, and Y + // axes with specific angle modifications based on the sensor's reported Euler + // angles (ins->euler_angles().x(), ins->euler_angles().y(), + // ins->euler_angles().z()). The exact meaning of sensor's Euler angles (e.g., + // order, positive direction, reference frame) and the reason for these + // specific transformations (e.g., -90 deg yaw offset, negating pitch, + // swapping axes in Eigen AngleAxisd) should be verified against the sensor + // documentation and Apollo's coordinate conventions. gps_msg->mutable_orientation()->set_qx(q.x()); gps_msg->mutable_orientation()->set_qy(q.y()); gps_msg->mutable_orientation()->set_qz(q.z()); gps_msg->mutable_orientation()->set_qw(q.w()); + // 3. Linear velocity + // Assuming linear velocity in vehicle body frame (X-forward, Y-left, Z-up) + // Novatel often reports velocity in the NED or ENU frame. + // Original code copies directly: Assuming sensor velocity is already in + // vehicle body frame. If sensor reports velocity in NED/ENU, it needs + // rotation by the vehicle's orientation quaternion. Let's assume for now + // sensor velocity is compatible or this transformation is done elsewhere. + // Comment: Assuming sensor reports linear velocity in the target vehicle body + // frame. gps_msg->mutable_linear_velocity()->set_x(ins->linear_velocity().x()); gps_msg->mutable_linear_velocity()->set_y(ins->linear_velocity().y()); gps_msg->mutable_linear_velocity()->set_z(ins->linear_velocity().z()); - gps_writer_->Write(gps); + gps_writer_->Write(gps); // Using shared_ptr for created message if (config_.tf().enable()) { TransformStamped transform; + // Pass by reference as GpsToTransformStamped modifies the object GpsToTransformStamped(gps, &transform); tf_broadcaster_.SendTransform(transform); } } -void DataParser::PublishCorrimu(const MessagePtr message) { +void DataParser::PublishCorrimu(const Parser::ProtoMessagePtr message) { Ins *ins = As(message); + if (!ins) { + AERROR << "Failed to cast message to Ins for Corrimu"; + return; + } + // Create a *new* CorrectedImu message auto imu = std::make_shared(); double unix_sec = apollo::drivers::util::gps2unix(ins->measurement_time()); imu->mutable_header()->set_timestamp_sec(unix_sec); auto *imu_msg = imu->mutable_imu(); - imu_msg->mutable_linear_acceleration()->set_x( - -ins->linear_acceleration().y()); - imu_msg->mutable_linear_acceleration()->set_y(ins->linear_acceleration().x()); - imu_msg->mutable_linear_acceleration()->set_z(ins->linear_acceleration().z()); - imu_msg->mutable_angular_velocity()->set_x(-ins->angular_velocity().y()); - imu_msg->mutable_angular_velocity()->set_y(ins->angular_velocity().x()); - imu_msg->mutable_angular_velocity()->set_z(ins->angular_velocity().z()); - - imu_msg->mutable_euler_angles()->set_x(ins->euler_angles().x()); - imu_msg->mutable_euler_angles()->set_y(-ins->euler_angles().y()); - imu_msg->mutable_euler_angles()->set_z(ins->euler_angles().z() - - 90 * DEG_TO_RAD_LOCAL); + // --- Coordinate System Transformation (Example: Novatel INS to Apollo + // Vehicle) --- Similar to PublishImu, transforming linear acceleration and + // angular velocity based on assumed sensor vs. target frame conventions. + // Verify these transformations against sensor documentation and Apollo + // conventions. + imu_msg->mutable_linear_acceleration()->set_x( + -ins->linear_acceleration() + .y()); // Sensor +Y (right) becomes Target -X (forward) + imu_msg->mutable_linear_acceleration()->set_y( + ins->linear_acceleration() + .x()); // Sensor +X (forward) becomes Target +Y (left) + imu_msg->mutable_linear_acceleration()->set_z( + ins->linear_acceleration() + .z()); // Sensor +Z (down) becomes Target +Z (up) - Check sign + + imu_msg->mutable_angular_velocity()->set_x( + -ins->angular_velocity() + .y()); // Sensor +Y (right) becomes Target -X (roll) + imu_msg->mutable_angular_velocity()->set_y( + ins->angular_velocity() + .x()); // Sensor +X (forward) becomes Target +Y (pitch) + imu_msg->mutable_angular_velocity()->set_z( + ins->angular_velocity() + .z()); // Sensor +Z (down) becomes Target +Z (yaw) - Check sign + + // --- Euler Angle Transformation --- + // Applying specific transformations to Euler angles. + // The meaning of sensor's Euler angles (ins->euler_angles().x/.y/.z) and + // the reason for negation and yaw offset should be verified. + // Assuming sensor angles are Roll(X), Pitch(Y), Yaw(Z). + imu_msg->mutable_euler_angles()->set_x( + ins->euler_angles().x()); // Roll (kept as is) + imu_msg->mutable_euler_angles()->set_y( + -ins->euler_angles().y()); // Pitch (negated) + imu_msg->mutable_euler_angles()->set_z( + ins->euler_angles().z() - // Yaw (-90 deg offset) + 90 * DEG_TO_RAD_LOCAL); + // Comment: The Euler angles are transformed based on sensor specifications + // and target frame requirements. Specifically, pitch is negated and a -90 + // degree offset is applied to the yaw angle. Verify this transformation + // against sensor documentation and Apollo's coordinate conventions. corrimu_writer_->Write(imu); } -void DataParser::PublishEphemeris(const MessagePtr message) { - auto eph = std::make_shared(*As(message)); - gnssephemeris_writer_->Write(eph); +void DataParser::PublishEphemeris(const Parser::ProtoMessagePtr message) { + GnssEphemeris *eph_ptr = As(message); + if (!eph_ptr) { + AERROR << "Failed to cast message to GnssEphemeris"; + return; + } + GnssEphemeris eph = *eph_ptr; + // Ephemeris message often doesn't have standard header, but can fill if + // needed common::util::FillHeader("gnss", &eph); + gnssephemeris_writer_->Write(eph); // Use const& overload } -void DataParser::PublishObservation(const MessagePtr message) { - auto observation = - std::make_shared(*As(message)); +void DataParser::PublishObservation(const Parser::ProtoMessagePtr message) { + // Get the underlying protobuf message + EpochObservation *observation_ptr = As(message); + if (!observation_ptr) { + AERROR << "Failed to cast message to EpochObservation"; + return; + } + // Create message directly and publish + EpochObservation observation = *observation_ptr; + // Observation message often doesn't have standard header, but can fill if + // needed common::util::FillHeader("gnss", &observation); epochobservation_writer_->Write(observation); } -void DataParser::PublishHeading(const MessagePtr message) { - auto heading = std::make_shared(*As(message)); - heading_writer_->Write(heading); +void DataParser::PublishHeading(const Parser::ProtoMessagePtr message) { + // Get the underlying protobuf message + Heading *heading_ptr = As(message); + if (!heading_ptr) { + AERROR << "Failed to cast message to Heading"; + return; + } + // Create message directly and publish + Heading heading = *heading_ptr; + common::util::FillHeader("gnss", &heading); + heading_writer_->Write(heading); // Use const& overload } void DataParser::GpsToTransformStamped(const std::shared_ptr &gps, TransformStamped *transform) { + CHECK_NOTNULL(gps); + CHECK_NOTNULL(transform); + transform->mutable_header()->set_timestamp_sec(gps->header().timestamp_sec()); transform->mutable_header()->set_frame_id(config_.tf().frame_id()); transform->set_child_frame_id(config_.tf().child_frame_id()); diff --git a/modules/drivers/gnss/parser/data_parser.h b/modules/drivers/gnss/parser/data_parser.h index ccd73d2c68b..b4d5ba89b63 100644 --- a/modules/drivers/gnss/parser/data_parser.h +++ b/modules/drivers/gnss/parser/data_parser.h @@ -17,26 +17,30 @@ #pragma once #include +#include #include +#include +#include -#define ACCEPT_USE_OF_DEPRECATED_PROJ_API_H -#include +// Use modern PROJ API +#include -#include "cyber/cyber.h" -#include "modules/transform/transform_broadcaster.h" +#include -#include "modules/drivers/gnss/proto/config.pb.h" +#include "modules/common_msgs/localization_msgs/gps.pb.h" +#include "modules/common_msgs/localization_msgs/imu.pb.h" #include "modules/common_msgs/sensor_msgs/gnss.pb.h" #include "modules/common_msgs/sensor_msgs/gnss_best_pose.pb.h" #include "modules/common_msgs/sensor_msgs/gnss_raw_observation.pb.h" -#include "modules/drivers/gnss/proto/gnss_status.pb.h" #include "modules/common_msgs/sensor_msgs/heading.pb.h" #include "modules/common_msgs/sensor_msgs/imu.pb.h" #include "modules/common_msgs/sensor_msgs/ins.pb.h" -#include "modules/common_msgs/localization_msgs/gps.pb.h" -#include "modules/common_msgs/localization_msgs/imu.pb.h" +#include "modules/drivers/gnss/proto/config.pb.h" +#include "modules/drivers/gnss/proto/gnss_status.pb.h" +#include "cyber/cyber.h" #include "modules/drivers/gnss/parser/parser.h" +#include "modules/transform/transform_broadcaster.h" namespace apollo { namespace drivers { @@ -44,41 +48,86 @@ namespace gnss { class DataParser { public: - using MessagePtr = ::google::protobuf::Message *; + // MessageMap is used to store raw byte messages like GPGGA + // std::vector is used to hold the raw data. + // Note: TryGetMessage is marked thread unsafe. Access to this map + // might need synchronization depending on usage context. + using MessageMap = + std::unordered_map>; + DataParser(const config::Config &config, const std::shared_ptr &node); - ~DataParser() {} + + // Destructor to release PROJ resources + ~DataParser(); + + // Initializes the parser and writers. Returns true on success. bool Init(); + + // Parses raw data string. Assumes msg contains data from the GNSS/IMU device. void ParseRawData(const std::string &msg); + // Get the parsed raw message data for a specific type. + // Currently only used for raw GPGGA data. + // Thread unsafe - direct access to message_map_. + std::optional> TryGetMessage( + const Parser::MessageType &type) { + if (auto it = message_map_.find(type); it != message_map_.end()) { + return it->second; + } + return std::nullopt; + } + private: - void DispatchMessage(Parser::MessageType type, MessagePtr message); - void PublishInsStat(const MessagePtr message); - void PublishOdometry(const MessagePtr message); - void PublishCorrimu(const MessagePtr message); - void PublishImu(const MessagePtr message); - void PublishBestpos(const MessagePtr message); - void PublishEphemeris(const MessagePtr message); - void PublishObservation(const MessagePtr message); - void PublishHeading(const MessagePtr message); + // Dispatches parsed Protobuf messages to the appropriate handling function. + void DispatchMessage(Parser::MessageType type, + Parser::ProtoMessagePtr message); + + // Publishes specific message types. Using const& or passing by value/rvalue + // where possible to avoid unnecessary shared_ptr creation for messages + // not requiring shared ownership within this class. + void PublishInsStat(const Parser::ProtoMessagePtr message); + void PublishOdometry(const Parser::ProtoMessagePtr message); + void PublishCorrimu(const Parser::ProtoMessagePtr message); + void PublishImu(const Parser::ProtoMessagePtr message); + void PublishBestpos(const Parser::ProtoMessagePtr message); + void PublishEphemeris(const Parser::ProtoMessagePtr message); + void PublishObservation(const Parser::ProtoMessagePtr message); + void PublishHeading(const Parser::ProtoMessagePtr message); + + // Checks and updates internal status based on Ins message. void CheckInsStatus(Ins *ins); + + // Checks and updates internal status based on Gnss message. void CheckGnssStatus(Gnss *gnss); + + // Converts Gps message to TransformStamped for TF broadcasting. void GpsToTransformStamped( const std::shared_ptr &gps, apollo::transform::TransformStamped *transform); bool init_flag_ = false; config::Config config_; - std::unique_ptr data_parser_; + std::unique_ptr gnss_parser_; apollo::transform::TransformBroadcaster tf_broadcaster_; GnssStatus gnss_status_; InsStatus ins_status_; uint32_t ins_status_record_ = static_cast(0); - projPJ wgs84pj_source_; - projPJ utm_target_; + + // PROJ objects for coordinate transformation. Use PJ* for modern API. + PJ *wgs84pj_source_ = nullptr; // WGS84 Geographic (Lat/Lon) + PJ *utm_target_ = nullptr; // Configured target projection (e.g., UTM) + + // Map to store raw messages (currently only GPGGA). See comments for + // MessageMap type. + MessageMap message_map_; std::shared_ptr node_ = nullptr; + + // Cyber writers for various message types. + // Initialized in Init(). Use shared_ptr as Cyber Writers are shared + // resources. std::shared_ptr> gnssstatus_writer_ = nullptr; std::shared_ptr> insstatus_writer_ = nullptr; diff --git a/modules/drivers/gnss/parser/huace/BUILD b/modules/drivers/gnss/parser/huace/BUILD new file mode 100644 index 00000000000..9c57f8a7d02 --- /dev/null +++ b/modules/drivers/gnss/parser/huace/BUILD @@ -0,0 +1,27 @@ +load("@rules_cc//cc:defs.bzl", "cc_library") +load("//tools:cpplint.bzl", "cpplint") + +cc_library( + name = "huace_parser", + srcs = [ + "huace_parser.cc", + "huace_messages.h", + ], + hdrs = [ + "huace_parser.h", + ], + deps = [ + ":parser", + ":data_buffer", + "//cyber", + "//modules/common_msgs/sensor_msgs:gnss_best_pose_cc_proto", + "//modules/common_msgs/sensor_msgs:imu_cc_proto", + "//modules/common_msgs/localization_msgs:ins_cc_proto", + "//modules/drivers/gnss/proto:gnss_status_cc_proto", + "//modules/common_msgs/localization_msgs:heading_cc_proto", + "//modules/common/util:util_tool", + "//modules/drivers/gnss/util:gnss_util", + ], +) + +cpplint() diff --git a/modules/drivers/gnss/parser/huace/huace_messages.h b/modules/drivers/gnss/parser/huace/huace_messages.h new file mode 100644 index 00000000000..d7f1eb20251 --- /dev/null +++ b/modules/drivers/gnss/parser/huace/huace_messages.h @@ -0,0 +1,193 @@ +// Copyright 2024 daohu527@gmail.com +// +// 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. + +// Created Date: 2024-12-30 +// Author: daohu527 + +#pragma once + +#include +#include + +namespace apollo { +namespace drivers { +namespace gnss { +namespace huace { + +// Add these constants to your header or appropriate .cc file +// (Assuming FRAME_TERMINATOR is already defined as "\r\n") +const size_t NUMA_CRC_LENGTH = 2; +const uint8_t NMEA_CHECKSUM_DELIMITER = '*'; + +// Define the expected number of fields in the comma-separated part of GPCHCX +// Count the fields in GPCHCX struct *excluding* header and checksum. +// gps_week_number, seconds_in_gps_week, heading_deg, ..., status, +// differential_age_ms, warning_flags (18 fields from Base) +// + latitude_std_m, ..., heading_std_deg (9 fields) +// + speed_heading_deg, speed_heading_std_deg (2 fields) +// + antenna_x_m, ..., direction_angle_deg (4 fields) +// + device_sn (1 field) +// Total: 18 + 9 + 2 + 4 + 1 = 34 fields *after* the header. +constexpr size_t GPCHCX_FIELD_COUNT = 34; + +enum class SystemStatus : uint8_t { + INIT = 0x00, + GUIDANCE = 0x01, + COMBINED = 0x02, + INERTIAL = 0x03 +}; + +enum class SatelliteStatus : uint8_t { + NO_POS_NO_ORIENT = 0x00, + SINGLE_POS_ORIENT = 0x01, + PSEUDORANGE_DIFF_ORIENT = 0x02, + COMBINED_PREDICTION = 0x03, + RTK_STABLE_ORIENT = 0x04, + RTK_FLOAT_ORIENT = 0x05, + SINGLE_POS_NO_ORIENT = 0x06, + PSEUDORANGE_DIFF_NO_ORIENT = 0x07, + RTK_STABLE_NO_ORIENT = 0x08, + RTK_FLOAT_NO_ORIENT = 0x09 +}; + +struct Status { + SystemStatus system : 4; + SatelliteStatus satellite : 4; +}; + +struct GPCHCBase { + // GPCHC protocol header, default value "$GPCHC" + static constexpr char header_gpchc[] = "$GPCHC"; + // GPS week number since 1980-1-6 + uint32_t gps_week_number = 0; + // Seconds since the start of the current GPS week + double seconds_in_gps_week = 0.0; + // Heading angle (0 to 359.99) in degrees + double heading_deg = 0.0; + // Pitch angle (-90 to 90) in degrees + double pitch_deg = 0.0; + // Roll angle (-180 to 180) in degrees + double roll_deg = 0.0; + // Gyroscope X-axis + double gyro_x = 0.0; + // Gyroscope Y-axis + double gyro_y = 0.0; + // Gyroscope Z-axis + double gyro_z = 0.0; + // Accelerometer X-axis + double acc_x = 0.0; + // Accelerometer Y-axis + double acc_y = 0.0; + // Accelerometer Z-axis + double acc_z = 0.0; + // Latitude (-90° to 90°) in degrees + double latitude_deg = 0.0; + // Longitude (-180° to 180°) in degrees + double longitude_deg = 0.0; + // Altitude in meters + double altitude_m = 0.0; + // Eastward velocity in m/s + double velocity_east_mps = 0.0; + // Northward velocity in m/s + double velocity_north_mps = 0.0; + // Upward velocity in m/s + double velocity_up_mps = 0.0; + // Vehicle speed in m/s + double speed_mps = 0.0; + // Number of satellites for main antenna + uint8_t main_antenna_satellite_count = 0; + // Number of satellites for secondary antenna + uint8_t secondary_antenna_satellite_count = 0; + Status status = {}; // Initialize Status members + // Differential delay + uint32_t differential_age_ms = 0; + // Warning flags: + union { + uint8_t warning_flags = 0; // Initialize union member + struct { + uint8_t no_gps_message : 1; + uint8_t no_vehicle_message : 1; + uint8_t reserved2 : 1; + uint8_t gyroscope_error : 1; + uint8_t accelerometer_error : 1; + uint8_t reserved5 : 3; + }; + }; +}; + +struct GPCHC : public GPCHCBase {}; + +struct GPCHCX : public GPCHCBase { + // GPCHCX protocol header, default value "$GPCHCX" + static constexpr char header[] = "$GPCHCX"; + + // Latitude standard deviation, unit (meters) + double latitude_std_m = 0.0; + // Longitude standard deviation, unit (meters) + double longitude_std_m = 0.0; + // Altitude standard deviation, unit (meters) + double altitude_std_m = 0.0; + // Eastward velocity standard deviation, unit (m/s) + double velocity_east_std_mps = 0.0; + // Northward velocity standard deviation, unit (m/s) + double velocity_north_std_mps = 0.0; + // Upward velocity standard deviation, unit (m/s) + double velocity_up_std_mps = 0.0; + + // Roll angle standard deviation, unit (degrees) + double roll_std_deg = 0.0; + // Pitch angle standard deviation, unit (degrees) + double pitch_std_deg = 0.0; + // Heading angle standard deviation, unit (degrees) + double heading_std_deg = 0.0; + + // Separator symbol "X" for easy parsing and reference (not a field in NMEA + // structure) char separator = 'X'; // This is not typically part of the + // comma-separated fields + + // Speed heading (0 to 359.99) in degrees (accurate to two decimal places) + double speed_heading_deg = 0.0; + // Speed heading standard deviation, unit (degrees), accurate to two decimal + // places + double speed_heading_std_deg = 0.0; + // Antenna position X-axis lever arm relative to the device, in vehicle + // coordinate system, unit (meters), accurate to two decimal places + float antenna_x_m = 0.0f; + // Antenna position Y-axis lever arm relative to the device, in vehicle + // coordinate system, unit (meters), accurate to two decimal places + float antenna_y_m = 0.0f; + // Antenna position Z-axis lever arm relative to the device, in vehicle + // coordinate system, unit (meters), accurate to two decimal places + float antenna_z_m = 0.0f; + // Rotation Euler angle from device coordinate system to vehicle coordinate + // system, X-axis angle, unit (degrees), accurate to two decimal places + float rotation_angle_x_deg = 0.0f; + // Rotation Euler angle from device coordinate system to vehicle coordinate + // system, Y-axis angle, unit (degrees), accurate to two decimal places + float rotation_angle_y_deg = 0.0f; + // Rotation Euler angle from device coordinate system to vehicle coordinate + // system, Z-axis angle, unit (degrees), accurate to two decimal places + float rotation_angle_z_deg = 0.0f; + // Rotation angle from vehicle heading to GNSS heading direction, along + // vehicle coordinate system Z-axis, unit (degrees), accurate to two decimal + // places + float direction_angle_deg = 0.0f; + // Device serial number (e.g., 6 chars + null terminator) + std::array device_sn = {}; // Initialize array with zeros/nulls +}; + +} // namespace huace +} // namespace gnss +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/gnss/parser/huace/huace_parser.cc b/modules/drivers/gnss/parser/huace/huace_parser.cc new file mode 100644 index 00000000000..2414ba17730 --- /dev/null +++ b/modules/drivers/gnss/parser/huace/huace_parser.cc @@ -0,0 +1,1137 @@ +// Copyright 2024 daohu527@gmail.com +// +// 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. + +// Created Date: 2024-12-30 +// Author: daohu527 + +#include "modules/drivers/gnss/parser/huace/huace_parser.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cyber/common/log.h" +#include "modules/common/util/util.h" +#include "modules/drivers/gnss/util/util.h" + +namespace { + +SolutionStatus ToSolutionStatus(huace::Status status) { + switch (status) { + case huace::Status::RTK_STABLE_ORIENT: + return SolutionStatus::INS_RTKFIXED; + case huace::Status::SINGLE_POS_NO_ORIENT: + return SolutionStatus::INSUFFICIENT_OBS; + case huace::Status::RTK_FLOAT_NO_ORIENT: + return SolutionStatus::NO_CONVERGENCE; + case huace::Status::NO_POS_NO_ORIENT: + return SolutionStatus::COLD_START; + case huace::Status::PSEUDORANGE_DIFF_NO_ORIENT: + return SolutionStatus::V_H_LIMIT; + case huace::Status::PSEUDORANGE_DIFF_ORIENT: + return SolutionStatus::INVALID_FIX; + case huace::Status::INIT: + return SolutionStatus::INVALID_RATE; + case huace::Status::SINGLE_POS_ORIENT: + return SolutionStatus::SINGLE; + case huace::Status::RTK_FLOAT_ORIENT: + return SolutionStatus::FLOATCONV; + case huace::Status::COMBINED_PREDICTION: + return SolutionStatus::RTK_DIRECT_INS; + // Add other huace::Status mappings + default: + AWARN << "Unhandled huace::Status value for SolutionStatus: " + << static_cast(status); + return SolutionStatus::UNKNOWN; + } +} + +// Helper functions to convert Huace status enum to Apollo's SolutionType. +// Robustly handles unknown status values by logging and returning UNKNOWN. +SolutionType ToSolutionType(huace::Status status) { + switch (status) { + case huace::Status::SINGLE_POS_ORIENT: + return SolutionType::FIXEDPOS; + case huace::Status::RTK_FLOAT_ORIENT: + return SolutionType::FLOATCONV; + case huace::Status::RTK_STABLE_ORIENT: + return SolutionType::WIDELANE; + case huace::Status::SINGLE_POS_NO_ORIENT: + return SolutionType::SINGLE; + case huace::Status::COMBINED_PREDICTION: + return SolutionType::RTK_DIRECT_INS; + case huace::Status::RTK_FLOAT_NO_ORIENT: + return SolutionType::PPP; + case huace::Status::PSEUDORANGE_DIFF_NO_ORIENT: + return SolutionType::DGPS; + case huace::Status::PSEUDORANGE_DIFF_ORIENT: + return SolutionType::DGPS; + case huace::Status::INIT: + return SolutionType::NONE; + case huace::Status::NO_POS_NO_ORIENT: + return SolutionType::NO_FIX; + // Add other huace::Status mappings + default: + AWARN << "Unhandled huace::Status value for SolutionType: " + << static_cast(status); + return SolutionType::UNKNOWN; + } +} + +// Hypothetical safe string-to-char-array helper. +// Assumes str_to_char_array handles target buffer size and null termination. +bool SafeStrToCharArray(const std::string& src, char* dest, size_t dest_size, + const char* field_name) { + if (dest == nullptr || dest_size == 0) { + AERROR << "SafeStrToCharArray: Invalid destination buffer for field: " + << field_name; + return false; // Invalid destination + } + + size_t chars_to_copy = std::min(src.length(), dest_size - 1); + src.copy(dest, chars_to_copy); + dest[chars_to_copy] = '\0'; // Ensure null termination + + if (src.length() > chars_to_copy) { + // Optionally log a warning if truncation occurred + AERROR << "Warning: Value for field '" << field_name << "' truncated from '" + << src << "' to '" << std::string(dest) << "' (max size " + << (dest_size - 1) << ")"; + } + + return true; // Copy successful (possibly with truncation) +} + +} // namespace + +namespace apollo { +namespace drivers { +namespace gnss { + +const std::unordered_map + HuaceParser::FRAME_HEADER_MAP = { + {"$GPCHC", FrameType::GPCHC}, + {"$GPCHCX", FrameType::GPCHCX}, + {"$GPGGA", FrameType::GAPPA} + // Add other headers to frame type mappings here +}; + +// ProcessHeader implementation for text protocol: find header and skip garbage. +bool HuaceParser::ProcessHeader() { + // Iterate through known headers to find the first match in the buffer + for (const auto& pair : FRAME_HEADER_MAP) { + // Assume DataBuffer::Search finds the first occurrence of the pattern + // and returns its starting position (offset from buffer start). + // This requires DataBuffer::Search(const std::string& pattern, size_t* + // found_pos). + size_t found_pos; + bool header_found = buffer_.Search(pair.first, &found_pos); + if (header_found) { + buffer_.Consume(found_pos); // Consume garbage data before header + // Header found. Set the current frame type and header size. + current_frame_type_ = pair.second; + current_header_size_ = pair.first.size(); + AINFO << "Header found: " << pair.first + << ", Frame Type: " << static_cast(current_frame_type_); + return true; // Header found and consumed + } + } + return false; +} + +// Calculates NMEA XOR checksum over a byte range [start, end) +uint8_t HuaceParser::CalculateNmeaChecksum(const std::vector& data, + size_t start, size_t end) { + uint8_t checksum = 0; + // Ensure the range is valid within the data vector + if (start > data.size() || end > data.size() || start > end) { + // Log an error or handle invalid range if necessary, + // but the caller (IsChecksumValid) should ideally prevent this. + AERROR << "CalculateNmeaChecksum: Invalid range [" << start << ", " << end + << ") for data size " << data.size(); + return 0; // Return 0 for invalid range, or consider throwing an exception + } + + // XOR all bytes from start up to (but not including) end + for (size_t i = start; i < end; ++i) { + checksum ^= data[i]; + } + return checksum; +} + +// Implementation for ParseHexByte +// Parses 2 hex characters from 'data' starting at 'start' into a single byte. +// Returns std::nullopt if characters are not valid hex digits or data is +// insufficient. +std::optional HuaceParser::ParseHexByte( + const std::vector& data, size_t start) { + // Need at least 2 bytes starting from 'start' + if (start + 1 >= data.size()) { + AERROR << "ParseHexByte: Not enough data for 2 hex characters starting at " + << start; + return std::nullopt; + } + + // Extract the two characters + char hex_chars[3]; // Need space for 2 characters + null terminator + hex_chars[0] = static_cast(data[start]); + hex_chars[1] = static_cast(data[start + 1]); + hex_chars[2] = '\0'; // Null terminate the string + + // Use std::strtol to parse the hex string + char* end_ptr; + // Parse as a long in base 16 (hexadecimal) + long value = std::strtol(hex_chars, &end_ptr, 16); + + // Check for parsing errors: + // 1. Check if strtol successfully parsed 2 characters (end_ptr should point + // to the null terminator) + // 2. Check if the parsed value is within the valid range for a uint8_t + // (0-255) + if (*end_ptr != '\0' || value < 0 || + value > std::numeric_limits::max()) { + AERROR << "ParseHexByte: Failed to parse '" << hex_chars + << "' as hex. Invalid format or range."; + return std::nullopt; // Invalid hex string or out of range + } + + // Conversion successful, return the value as uint8_t + return static_cast(value); +} + +// Modified ProcessPayload +std::optional> HuaceParser::ProcessPayload() { + // We are in PROCESS_PAYLOAD state, meaning ProcessHeader found a header + // and the buffer currently starts with that header. + // We need to find the frame terminator (\r\n) to get the full frame. + + size_t terminator_pos; + // Assume DataBuffer::Search finds the pattern from the current buffer start. + if (!buffer_.Search(huace::FRAME_TERMINATOR, &terminator_pos)) { + // Terminator not found in the current buffer content. Need more data. + AINFO_IF(!buffer_.IsEmpty()) + << "Huace frame terminator '" << huace::FRAME_TERMINATOR + << "' not found. Need more data."; + return std::nullopt; + } + + // Terminator found. The complete frame includes header, payload, '*', CRC, + // and terminator. Total frame length is terminator_pos + + // huace::FRAME_TERMINATOR.size(). + size_t total_frame_length = terminator_pos + huace::FRAME_TERMINATOR.size(); + + // --- Calculate payload and checksum bounds based on fixed format --- + // Format: Header + Payload + '*' + CRC (2 hex chars) + "\r\n" + // CRC characters are NUMA_CRC_LENGTH bytes long. + // The '*' delimiter is 1 byte. + // Terminator is huace::FRAME_TERMINATOR.size() bytes. + + // Minimum required length: Header + '*' + CRC + Terminator + const size_t min_required_payload_bytes = 0; // Payload can be empty + const size_t min_frame_size = current_header_size_ + 1 /* '*' */ + + NUMA_CRC_LENGTH + + huace::FRAME_TERMINATOR.size(); + + if (total_frame_length < min_frame_size) { + AERROR << "Frame data too short for expected format (min size: " + << min_frame_size << ", actual: " << total_frame_length + << "). Consuming frame."; + buffer_.Consume(total_frame_length); // Consume malformed frame + return std::vector(); + } + + // Position of the '*' character (just before CRC hex chars and terminator) + size_t checksum_delimiter_pos = terminator_pos - NUMA_CRC_LENGTH - 1; + + // Position of the first CRC hex character + size_t crc_chars_start_pos = terminator_pos - NUMA_CRC_LENGTH; + + // Position of the byte *after* the last CRC hex character (before \r\n) + size_t crc_chars_end_pos = terminator_pos; // terminator_pos is index of '\r' + + // Payload data is from after the header up to the '*' delimiter + size_t payload_start_pos = current_header_size_; + // Payload ends just before '*' + size_t payload_end_pos = checksum_delimiter_pos; + + // Check if '*' is actually at the expected position + std::vector frame_data_peek = buffer_.Peek(0, total_frame_length); + if (checksum_delimiter_pos >= frame_data_peek.size() || + frame_data_peek[checksum_delimiter_pos] != NMEA_CHECKSUM_DELIMITER) { + AERROR << "Checksum delimiter '" + << static_cast(NMEA_CHECKSUM_DELIMITER) + << "' not found at expected position " << checksum_delimiter_pos + << ". Consuming frame."; + buffer_.Consume(total_frame_length); // Consume malformed frame + return std::vector(); + } + + // Validate checksum. + bool checksum_ok = IsChecksumValid( + frame_data_peek, // Full frame data + payload_start_pos, // Start of bytes to checksum over + payload_end_pos, // End of bytes to checksum over (exclusive) + crc_chars_start_pos, // Start of checksum hex characters + crc_chars_end_pos, // End of checksum hex characters (exclusive) + current_frame_type_); // Frame type might still be useful for + + // Consume the frame data REGARDLESS of checksum validity. + // This prevents a bad frame from blocking the parser. + buffer_.Consume(total_frame_length); + + if (!checksum_ok) { + AERROR << "Checksum validation failed for Huace frame. Consuming frame."; + // Data was consumed, but parsing failed. Return an empty vector to indicate + // no messages parsed from this frame. + return std::vector(); + } + + // Checksum is valid. Now parse the payload based on the frame type found in + // ProcessHeader. Pass the frame data and payload boundaries. + std::vector messages; + switch (current_frame_type_) { + case FrameType::GPCHC: + messages = + ParseGPCHC(frame_data_peek, payload_start_pos, payload_end_pos); + break; + case FrameType::GPCHCX: + messages = + ParseGPCHCX(frame_data_peek, payload_start_pos, payload_end_pos); + break; + case FrameType::GAPPA: + messages = + ParseGAPPA(frame_data_peek, payload_start_pos, payload_end_pos); + break; + default: + AERROR << "Unknown frame type (" << static_cast(current_frame_type_) + << ") in ProcessPayload after checksum validation."; + // Data was consumed, but parsing failed due to unknown type. + return std::vector(); + } + + // After successful parsing (even if Parse* returned an empty vector due to + // internal errors), the state is reset to SEEK_HEADER by TryParseMessage. + return messages; +} + +bool ParseFieldsToStruct(const std::vector& items, GPCHCX* out) { + CHECK_NOTNULL(out); + + // Zero out the struct initially to ensure a clean state on any failure + memset(out, 0, sizeof(*out)); + + // Use a lambda to simplify standard field parsing with error handling + // Added field_index for more specific error reporting + auto parse_and_assign_standard = + [&](const std::string& field_str, const char* field_name, + size_t field_index, + auto& target, // Use auto& for the target member reference + auto + simple_absl_parse_func  // e.g., absl::SimpleAtoi, absl::SimpleAtod + ) { + if (!simple_absl_parse_func(field_str, &target)) { + AERROR << "Failed to parse '" << field_str + << "' for field: " << field_name << " at index " << field_index + << " (expected " << typeid(target).name() << ")."; + return false; + } + return true; + }; + + // The index in this vector should correspond to the index in the 'items' + // vector *after* the header. + std::vector parsers = { + // GPS_WEEK_NUMBER field (index 0 after header) + {"gps_week_number", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "gps_week_number", 0, + out_ptr->gps_week_number, + absl::SimpleAtoi); + }}, + {"seconds_in_gps_week", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "seconds_in_gps_week", 1, + out_ptr->seconds_in_gps_week, + absl::SimpleAtod); + }}, + {"heading_deg", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard( + s, "heading_deg", 2, out_ptr->heading_deg, absl::SimpleAtod); + }}, + {"pitch_deg", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "pitch_deg", 3, out_ptr->pitch_deg, + absl::SimpleAtod); + }}, + {"roll_deg", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "roll_deg", 4, out_ptr->roll_deg, + absl::SimpleAtod); + }}, + {"gyro_x", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "gyro_x", 5, out_ptr->gyro_x, + absl::SimpleAtod); + }}, + {"gyro_y", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "gyro_y", 6, out_ptr->gyro_y, + absl::SimpleAtod); + }}, + {"gyro_z", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "gyro_z", 7, out_ptr->gyro_z, + absl::SimpleAtod); + }}, + {"acc_x", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "acc_x", 8, out_ptr->acc_x, + absl::SimpleAtod); + }}, + {"acc_y", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "acc_y", 9, out_ptr->acc_y, + absl::SimpleAtod); + }}, + {"acc_z", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "acc_z", 10, out_ptr->acc_z, + absl::SimpleAtod); + }}, + {"latitude_deg", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard( + s, "latitude_deg", 11, out_ptr->latitude_deg, absl::SimpleAtod); + }}, + {"longitude_deg", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard( + s, "longitude_deg", 12, out_ptr->longitude_deg, absl::SimpleAtod); + }}, + {"altitude_m", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard( + s, "altitude_m", 13, out_ptr->altitude_m, absl::SimpleAtod); + }}, + {"velocity_east_mps", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "velocity_east_mps", 14, + out_ptr->velocity_east_mps, + absl::SimpleAtod); + }}, + {"velocity_north_mps", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "velocity_north_mps", 15, + out_ptr->velocity_north_mps, + absl::SimpleAtod); + }}, + {"velocity_up_mps", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "velocity_up_mps", 16, + out_ptr->velocity_up_mps, + absl::SimpleAtod); + }}, + {"speed_mps", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "speed_mps", 17, + out_ptr->speed_mps, absl::SimpleAtod); + }}, + {"main_antenna_satellite_count", + [&](const std::string& s, GPCHCX* out_ptr) { + unsigned int tmp; // Use unsigned int for parsing + if (!absl::SimpleAtoi(s, &tmp)) { + AERROR << "Failed to parse '" << s + << "' as unsigned int for main_antenna_satellite_count at " + "index 18."; + return false; + } + if (tmp > std::numeric_limits::max()) { + AERROR << "Value " << tmp + << " for main_antenna_satellite_count at index 18 exceeds " + "uint8_t max."; + return false; + } + out_ptr->main_antenna_satellite_count = static_cast(tmp); + return true; + }}, + {"secondary_antenna_satellite_count", + [&](const std::string& s, GPCHCX* out_ptr) { + unsigned int tmp; // Use unsigned int for parsing + if (!absl::SimpleAtoi(s, &tmp)) { + AERROR << "Failed to parse '" << s + << "' as unsigned int for secondary_antenna_satellite_count " + "at index 19."; + return false; + } + if (tmp > std::numeric_limits::max()) { + AERROR << "Value " << tmp + << " for secondary_antenna_satellite_count at index 19 " + "exceeds uint8_t max."; + return false; + } + out_ptr->secondary_antenna_satellite_count = static_cast(tmp); + return true; + }}, + // Status field - manual unpacking based on bitfield definition (index 20) + {"status", + [&](const std::string& s, GPCHCX* out_ptr) { + uint32_t raw; // Parse the raw integer value + if (!absl::SimpleAtoi(s, &raw)) { + AERROR << "Failed to parse status string '" << s + << "' as uint32_t at index 20."; + return false; + } + // Add range check for the raw value itself + if (raw > std::numeric_limits::max()) { + AERROR << "Raw status value " << raw + << " at index 20 exceeds uint8_t max (255). Potentially " + "incorrect format or definition mismatch."; + return false; + } + + // Manual unpacking based on Status struct definition (system:4, + // satellite:4) + out_ptr->status.system = static_cast((raw >> 4) & 0x0F); + out_ptr->status.satellite = static_cast(raw & 0x0F); + + // Optional: Validate if the extracted enum values are valid for the + // enums This requires checking if the resulting static_cast + // corresponds to a valid enum value. For simplicity, we assume the + // bitmasking results in values expected by the enums. + + return true; + }}, + // Differential age (index 21) + {"differential_age_ms", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "differential_age_ms", 21, + out_ptr->differential_age_ms, + absl::SimpleAtoi); + }}, + // Warning flags (index 22) - using union, assign to uint8_t member + {"warning_flags", + [&](const std::string& s, GPCHCX* out_ptr) { + unsigned int tmp; // Use unsigned int for parsing + if (!absl::SimpleAtoi(s, &tmp)) { + AERROR << "Failed to parse '" << s + << "' as unsigned int for warning_flags at index 22."; + return false; + } + if (tmp > std::numeric_limits::max()) { + AERROR << "Value " << tmp + << " for warning_flags at index 22 exceeds uint8_t max."; + return false; + } + // Assign to the union member + out_ptr->warning_flags = static_cast(tmp); + return true; + }}, + // --- Fields from GPCHCX structure --- (indices 23 to 33) + {"latitude_std_m", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "latitude_std_m", 23, + out_ptr->latitude_std_m, + absl::SimpleAtod); + }}, + {"longitude_std_m", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "longitude_std_m", 24, + out_ptr->longitude_std_m, + absl::SimpleAtod); + }}, + {"altitude_std_m", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "altitude_std_m", 25, + out_ptr->altitude_std_m, + absl::SimpleAtod); + }}, + {"velocity_east_std_mps", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "velocity_east_std_mps", 26, + out_ptr->velocity_east_std_mps, + absl::SimpleAtod); + }}, + {"velocity_north_std_mps", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "velocity_north_std_mps", 27, + out_ptr->velocity_north_std_mps, + absl::SimpleAtod); + }}, + {"velocity_up_std_mps", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "velocity_up_std_mps", 28, + out_ptr->velocity_up_std_mps, + absl::SimpleAtod); + }}, + {"roll_std_deg", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard( + s, "roll_std_deg", 29, out_ptr->roll_std_deg, absl::SimpleAtod); + }}, + {"pitch_std_deg", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard( + s, "pitch_std_deg", 30, out_ptr->pitch_std_deg, absl::SimpleAtod); + }}, + {"heading_std_deg", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "heading_std_deg", 31, + out_ptr->heading_std_deg, + absl::SimpleAtod); + }}, + {"speed_heading_deg", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "speed_heading_deg", 32, + out_ptr->speed_heading_deg, + absl::SimpleAtod); + }}, + {"speed_heading_std_deg", + [&](const std::string& s, GPCHCX* out_ptr) { + return parse_and_assign_standard(s, "speed_heading_std_deg", 33, + out_ptr->speed_heading_std_deg, + absl::SimpleAtod); + }}, + // Antenna parameters - parsing double then casting to float as per + // definition + // (indices 34 to 37) + {"antenna_x_m", + [&](const std::string& s, GPCHCX* out_ptr) { + double tmp; // Parse to double first + if (!absl::SimpleAtod(s, &tmp)) { + AERROR << "Failed to parse '" << s + << "' as double for antenna_x_m at index 34."; + return false; + } + out_ptr->antenna_x_m = + static_cast(tmp); // Cast to float as per definition + return true; + }}, + {"antenna_y_m", + [&](const std::string& s, GPCHCX* out_ptr) { + double tmp; + if (!absl::SimpleAtod(s, &tmp)) { + AERROR << "Failed to parse '" << s + << "' as double for antenna_y_m at index 35."; + return false; + } + out_ptr->antenna_y_m = static_cast(tmp); + return true; + }}, + {"antenna_z_m", + [&](const std::string& s, GPCHCX* out_ptr) { + double tmp; + if (!absl::SimpleAtod(s, &tmp)) { + AERROR << "Failed to parse '" << s + << "' as double for antenna_z_m at index 36."; + return false; + } + out_ptr->antenna_z_m = static_cast(tmp); + return true; + }}, + {"rotation_angle_x_deg", + [&](const std::string& s, GPCHCX* out_ptr) { + double tmp; + if (!absl::SimpleAtod(s, &tmp)) { + AERROR << "Failed to parse '" << s + << "' as double for rotation_angle_x_deg at index 37."; + return false; + } + out_ptr->rotation_angle_x_deg = static_cast(tmp); + return true; + }}, + {"rotation_angle_y_deg", + [&](const std::string& s, GPCHCX* out_ptr) { + double tmp; + if (!absl::SimpleAtod(s, &tmp)) { + AERROR << "Failed to parse '" << s + << "' as double for rotation_angle_y_deg at index 38."; + return false; + } + out_ptr->rotation_angle_y_deg = static_cast(tmp); + return true; + }}, + {"rotation_angle_z_deg", + [&](const std::string& s, GPCHCX* out_ptr) { + double tmp; + if (!absl::SimpleAtod(s, &tmp)) { + AERROR << "Failed to parse '" << s + << "' as double for rotation_angle_z_deg at index 39."; + return false; + } + out_ptr->rotation_angle_z_deg = static_cast(tmp); + return true; + }}, + {"direction_angle_deg", + [&](const std::string& s, GPCHCX* out_ptr) { + double tmp; + if (!absl::SimpleAtod(s, &tmp)) { + AERROR << "Failed to parse '" << s + << "' as double for direction_angle_deg at index 40."; + return false; + } + out_ptr->direction_angle_deg = static_cast(tmp); + return true; + }}, + // DEVICE_SN field (index 41) - uses SafeStrToCharArray + {"device_sn", + [&](const std::string& s, GPCHCX* out_ptr) { + // Use the received string 's' with the SafeStrToCharArray helper + // function. Corrected size: std::array needs size 7, + // SafeStrToCharArray handles null termination within that size. + return SafeStrToCharArray(s, out_ptr->device_sn.data(), + out_ptr->device_sn.size(), "device_sn"); + }}, + // Add parsers for any subsequent fields if the protocol extends + }; + + // Check if we have the exact expected number of fields *after* the header. + // The total number of items including the header should be GPCHCX_FIELD_COUNT + // + 1. + if (items.size() != GPCHCX_FIELD_COUNT + 1) { + AERROR << "Incorrect number of fields in message. Expected " + << GPCHCX_FIELD_COUNT + 1 << " (including header), Got " + << items.size(); + // Struct was zeroed at the start, which is the desired state on failure. + return false; // Indicate failure due to incorrect field count + } + + // Validate the header string explicitly (items[0]) + const std::string& header_str = items[0]; + if (header_str != GPCHCX::header) { + AERROR << "Invalid header: Expected '" << GPCHCX::header << "', Got '" + << header_str << "'."; + // Struct was zeroed at the start. + return false; // Indicate failure due to invalid header + } + + // Now iterate through parsers and parse the corresponding item string, + // starting from index 1 in the items vector (after the header). + for (size_t i = 0; i < parsers.size(); ++i) { + // The item index is 1 + the parser index because items[0] is the header + size_t current_item_index = i + 1; + + // This check is mostly defensive given the items.size() check above, + // but ensures we don't go out of bounds if the logic was flawed. + if (current_item_index >= items.size()) { + AERROR << "Internal logic error: Parser index " << i + << " mapped to item index " << current_item_index + << ", but items size is only " << items.size() + << " for field: " << parsers[i].name; + // Struct was zeroed at the start. + return false; // Should not happen if size check passes + } + + const std::string& field_string = items[current_item_index]; + const char* field_name = parsers[i].name; + + // Execute the parser lambda for the current field + // The lambda for each field receives 'field_string' (which is + // items[current_item_index]) Pass the correct item index to the lambda for + // detailed error reporting. Note: The lambda `parse_and_assign_standard` + // was already modified to accept index. Now we just need to pass the index + // when calling the lambda from the parsers vector. The lambda in `parsers` + // vector needs to be updated to receive and pass index. Let's update the + // `FieldParser` definition and lambdas to include the index. + // *Self-correction*: The current lambda in `parsers` doesn't take the + // index. We need to update the `parse_and_assign_standard` call *inside* + // each parser lambda to pass 'current_item_index'. The lambda signature + // itself is fine as it just gets string and out_ptr. + + // Re-call parse_and_assign_standard with correct index inside each lambda. + // Let's refine the `parse_and_assign_standard` lambda and its usage again. + // The `parsers` vector stores lambdas of type `std::function`. The index logic should be *outside* this + // lambda, within the loop. The original `parse_and_assign_standard` lambda + // already takes field_index. The issue is that the lambdas *stored* in the + // `parsers` vector don't pass the index down. + + // Let's retry calling the stored lambda and ensure it correctly calls the + // helper lambda. The helper lambda `parse_and_assign_standard` is defined + // *outside* the `parsers` vector definition, so it's available to the + // lambdas inside the vector. + + // Corrected logic for calling the stored lambda: + if (!parsers[i].parse(field_string, out)) { + // The specific parsing error was already logged by the inner lambda + AERROR << "Parsing failed for field: " << field_name << " at item index " + << current_item_index << " (string: '" << field_string << "')"; + // Struct was zeroed at the start of this function. + return false; // Exit immediately on first parsing failure + } + // The individual lambda within 'parsers' is responsible for calling + // 'parse_and_assign_standard' with the correct index. + // Example: + // {"gps_week_number", [&](const std::string& s, GPCHCX* out_ptr) { + // return parse_and_assign_standard(s, "gps_week_number", 0, + // out_ptr->gps_week_number, absl::SimpleAtoi); // Index 0 here refers to + // the 0th field *after* header + // }}, + // The indices within the `parsers` vector lambdas (0, 1, 2, ...) + // should correspond to the index *relative to the start of the + // comma-separated fields*. Yes, the current lambdas in the `parsers` vector + // already do this (0 for gps_week_number, etc.). + } + + // If we reached here, all expected fields were successfully processed + return true; +} + +// Modified IsChecksumValid function signature and implementation +// Needs access to the full frame data, payload bounds for calculation, +// and CRC chars bounds for comparison. +bool HuaceParser::IsChecksumValid(const std::vector& frame_data, + size_t payload_start, size_t payload_end, + size_t crc_chars_start, size_t crc_chars_end, + FrameType type) { + // Check if CRC character bounds are valid + if (crc_chars_end <= crc_chars_start || crc_chars_end > frame_data.size() || + (crc_chars_end - crc_chars_start) != NUMA_CRC_LENGTH) { + AERROR << "Invalid CRC character bounds: [" << crc_chars_start << ", " + << crc_chars_end << ")"; + return false; + } + + // Calculate checksum over the payload bytes + uint8_t calculated_checksum = + CalculateNmeaChecksum(frame_data, payload_start, payload_end); + + // Parse the expected checksum from the hex characters + // The 2 hex chars are at crc_chars_start and crc_chars_start + 1 + std::optional expected_checksum_opt = + ParseHexByte(frame_data, crc_chars_start); + + if (!expected_checksum_opt) { + AERROR << "Failed to parse checksum hex characters at position " + << crc_chars_start; + return false; + } + uint8_t expected_checksum = *expected_checksum_opt; + + // Compare calculated and expected checksums + if (calculated_checksum != expected_checksum) { + AERROR << "Checksum mismatch. Calculated: " << std::hex + << static_cast(calculated_checksum) + << ", Expected: " << std::hex << static_cast(expected_checksum); + return false; + } + + // Checksum is valid + return true; +} + +std::vector HuaceParser::ParseGPCHC( + const std::vector& frame_data, size_t payload_start, + size_t payload_end) { + std::vector messages; // Return empty vector on failure + + // 1. Extract the payload byte range + // Be careful: std::string constructor from iterators is end-exclusive + std::string payload_str(frame_data.begin() + payload_start, + frame_data.begin() + payload_end); + + // 2. Use absl::StrSplit to split the payload string by comma + // Note: absl::StrSplit returns a vector of strings + std::vector items = absl::StrSplit(payload_str, ','); + + // 3. Parse common and GPCHCX specific fields + GPCHCX gpchcx; // Use GPCHCX struct for both + if (!ParseFieldsToStruct(items, &gpchcx)) { + // ParseFieldsToStruct logged the specific error + // Data was consumed in ProcessPayload (assumed), return empty vector + return parsed_messages; // Return empty vector on parsing failure + } + + // 3. Fill protobuf messages + // Create protobuf messages on the stack first + GnssBestPose bestpos; + Imu imu; + Ins ins; + InsStat ins_stat; + Heading heading; + + // Fill protobuf messages from the parsed struct + FillGnssBestpos(gpchcx, &bestpos); + FillImu(gpchcx, &imu); + FillHeading(gpchcx, &heading); + FillIns(gpchcx, &ins); + // CORRECTED: Pass the actual status from the parsed struct + FillInsStat(gpchcx.status, &ins_stat); + + // 4. Add filled messages to the result vector + // Use std::move for efficiency when creating unique_ptr from stack objects + parsed_messages.emplace_back( + MessageType::BEST_GNSS_POS, + std::make_unique(std::move(bestpos))); + parsed_messages.emplace_back(MessageType::IMU, + std::make_unique(std::move(imu))); + parsed_messages.emplace_back(MessageType::INS, + std::make_unique(std::move(ins))); + parsed_messages.emplace_back(MessageType::INS_STAT, + std::make_unique(std::move(ins_stat))); + parsed_messages.emplace_back(MessageType::HEADING, + std::make_unique(std::move(heading))); + return parsed_messages; +} + +std::vector HuaceParser::ParseGPCHCX( + const std::vector& frame_data, size_t payload_start, + size_t payload_end) { + std::vector messages; // Return empty vector on failure + + // 1. Extract the payload byte range + // Be careful: std::string constructor from iterators is end-exclusive + std::string payload_str(frame_data.begin() + payload_start, + frame_data.begin() + payload_end); + + // 2. Use absl::StrSplit to split the payload string by comma + // Note: absl::StrSplit returns a vector of strings + std::vector items = absl::StrSplit(payload_str, ','); + + // 3. Parse common and GPCHCX specific fields + GPCHCX gpchcx; // Use GPCHCX struct for both + if (!ParseFieldsToStruct(items, &gpchcx)) { + // ParseFieldsToStruct logged the specific error + // Data was consumed in ProcessPayload (assumed), return empty vector + return parsed_messages; // Return empty vector on parsing failure + } + + // 3. Fill protobuf messages + // Create protobuf messages on the stack first + GnssBestPose bestpos; + Imu imu; + Ins ins; + InsStat ins_stat; + Heading heading; + + // Fill protobuf messages from the parsed struct + FillGnssBestpos(gpchcx, &bestpos); + FillImu(gpchcx, &imu); + FillHeading(gpchcx, &heading); + FillIns(gpchcx, &ins); + // CORRECTED: Pass the actual status from the parsed struct + FillInsStat(gpchcx.status, &ins_stat); + + // 4. Add filled messages to the result vector + // Use std::move for efficiency when creating unique_ptr from stack objects + parsed_messages.emplace_back( + MessageType::BEST_GNSS_POS, + std::make_unique(std::move(bestpos))); + parsed_messages.emplace_back(MessageType::IMU, + std::make_unique(std::move(imu))); + parsed_messages.emplace_back(MessageType::INS, + std::make_unique(std::move(ins))); + parsed_messages.emplace_back(MessageType::INS_STAT, + std::make_unique(std::move(ins_stat))); + parsed_messages.emplace_back(MessageType::HEADING, + std::make_unique(std::move(heading))); + return parsed_messages; +} + +// Implement ParseGAPPA here (for raw GPGGA passthrough) +std::vector HuaceParser::ParseGAPPA( + const std::vector& frame_data, size_t payload_start, + size_t payload_end) { + std::vector parsed_messages; + // Assuming frame_data is the complete GPGGA frame from '$' to '\n'. + // For passthrough, we typically just wrap the raw bytes. + // Checksum validation for GAPPA/GPGGA should ideally use NMEA checksum logic + // (*CS\r\n), not the CRC32 logic in IsChecksumValid. If you need NMEA + // validation, implement a separate helper. For simplicity, we just wrap the + // raw data here as in the original code. + + parsed_messages.emplace_back(MessageType::GPGGA, frame_data); + + return parsed_messages; +} + +void HuaceParser::FillGnssBestpos(const GPCHCX& gpchcx, GnssBestPose* bestpos) { + // Note: Assuming gpchcx contains relevant fields populated by + // ParseGPCHC/ParseGPCHCX even if they were not explicitly in the original + // GPCHC definition (e.g., std deviations). This might require checking items + // vector size in ParseGPCHC and assigning defaults if fields are missing. + bestpos->set_measurement_time(gpchcx.gps_week * kSecondsPerWeek + + gpchcx.gps_time); + // Use mapping helpers + bestpos->set_sol_status( + ToSolutionStatus(static_cast(gpchcx.status_code))); + bestpos->set_sol_type( + ToSolutionType(static_cast(gpchcx.status_code))); + bestpos->set_latitude(gpchcx.latitude); + bestpos->set_longitude(gpchcx.longitude); + bestpos->set_height_msl(gpchcx.altitude); + // Fill standard deviations - these might only be available in GPCHCX, need to + // check + bestpos->set_latitude_std_dev(gpchcx.latitude_std); + bestpos->set_longitude_std_dev(gpchcx.longitude_std); + bestpos->set_height_std_dev(gpchcx.altitude_std); + + // Fill satellite counts + bestpos->set_num_sats_tracked(gpchcx.nsv1 + gpchcx.nsv2); + // Fields like num_sats_in_solution, num_sats_l1, num_sats_multi might need + // mapping from gpchcx or status code, check protocol spec. + // bestpos->set_num_sats_in_solution(...); +} + +void HuaceParser::FillImu(const GPCHCX& gpchcx, Imu* imu) { + imu->set_measurement_time(gpchcx.gps_week * kSecondsPerWeek + + gpchcx.gps_time); + + Point3D* linear_acceleration = imu->mutable_linear_acceleration(); + // Ensure correct coordinate transform (RFU to FLU) + rfu_to_flu(gpchcx.acc_x, gpchcx.acc_y, gpchcx.acc_z, linear_acceleration); + + Point3D* angular_velocity = imu->mutable_angular_velocity(); + // Ensure correct coordinate transform (RFU to FLU) + rfu_to_flu(gpchcx.gyro_x, gpchcx.gyro_y, gpchcx.gyro_z, angular_velocity); +} + +void HuaceParser::FillHeading(const GPCHCX& gpchcx, Heading* heading) { + heading->set_measurement_time(gpchcx.gps_week * kSecondsPerWeek + + gpchcx.gps_time); + // Use mapping helpers + heading->set_sol_status( + ToSolutionStatus(static_cast(gpchcx.status_code))); + heading->set_sol_type( + ToSolutionType(static_cast(gpchcx.status_code))); + heading->set_heading(gpchcx.heading); + heading->set_pitch(gpchcx.pitch); + // Fill standard deviations - might only be available in GPCHCX + heading->set_heading_std_dev(gpchcx.heading_std); + heading->set_pitch_std_dev(gpchcx.pitch_std); + + // Fields like station_id, satellite counts might need mapping + // heading->set_station_id("0"); // Default or map from gpchcx? + // heading->set_satellite_number_multi(...); + // heading->set_satellite_solution_number(...); +} + +void HuaceParser::FillIns(const GPCHCX& gpchcx, Ins* ins) { + // FIX: Use GPS time for Protobuf header timestamp as well + double gps_time_sec = gpchcx.gps_week * kSecondsPerWeek + gpchcx.gps_time; + ins->mutable_header()->set_timestamp_sec(gps_time_sec); // Use sensor time + + ins->set_measurement_time(gps_time_sec); + + // Map SolutionType to Ins::Type + SolutionType solution_type = + ToSolutionType(static_cast(gpchcx.status_code)); + switch (solution_type) { + // Map relevant SolutionTypes to Ins::Type + case SolutionType::INS_RTKFIXED: + case SolutionType::NARROW_INT: + case SolutionType::INS_RTKFLOAT: + case SolutionType::NARROW_FLOAT: + ins->set_type(Ins::GOOD); + break; + case SolutionType::SINGLE: + ins->set_type(Ins::CONVERGING); + break; + case SolutionType::WIDELANE: // Often implies float RTK + case SolutionType::FLOATCONV: // Explicit float RTK + ins->set_type(Ins::CONVERGING); // Or GOOD_FLOAT? Check Ins::Type options + break; + case SolutionType::RTK_DIRECT_INS: + ins->set_type(Ins::GOOD); + break; + default: + ins->set_type(Ins::INVALID); // Handle unknown or invalid statuses + break; + } + + apollo::common::Point3D* position = ins->mutable_position(); + position->set_lon(gpchcx.longitude); + position->set_lat(gpchcx.latitude); + position->set_height(gpchcx.altitude); + + apollo::common::Quaternion* euler_angles = ins->mutable_euler_angles(); + // Ensure correct mapping and units (Roll/Pitch/Heading vs Euler angles) + // Check coordinate system conversions (RFU to FLU, Azimuth to Yaw) + euler_angles->set_x(gpchcx.roll * DEG_TO_RAD); + // Check pitch sign convention based on Apollo FLU + euler_angles->set_y(-gpchcx.pitch * DEG_TO_RAD); + // Assuming heading is Azimuth 0-360 North=0, East=90 + // to Apollo Yaw + euler_angles->set_z(azimuth_deg_to_yaw_rad(gpchcx.heading)); + + apollo::common::Point3D* linear_velocity = ins->mutable_linear_velocity(); + // Check mapping of Ve/Vn/Vu to FLU velocity components (East, North, Up) -> + // (X, Y, Z) + linear_velocity->set_x(gpchcx.ve); + linear_velocity->set_y(gpchcx.vn); + linear_velocity->set_z(gpchcx.vu); + + // Assuming these are RFU gyro rates + Point3D* angular_velocity = ins->mutable_angular_velocity(); + rfu_to_flu(gpchcx.gyro_x, gpchcx.gyro_y, gpchcx.gyro_z, angular_velocity); + + Point3D* linear_acceleration = ins->mutable_linear_acceleration(); + // Assuming these are RFU accelerations + rfu_to_flu(gpchcx.acc_x, gpchcx.acc_y, gpchcx.acc_z, linear_acceleration); + + // Optional: Fill standard deviation fields if available in GPCHCX + // ins->mutable_position_std_dev()->set_lon(gpchcx.longitude_std); + // ins->mutable_position_std_dev()->set_lat(gpchcx.latitude_std); + // ins->mutable_position_std_dev()->set_height(gpchcx.altitude_std); + // ins->mutable_linear_velocity_std_dev()->set_x(gpchcx.ve_std); + // ins->mutable_linear_velocity_std_dev()->set_y(gpchcx.vn_std); + // ins->mutable_linear_velocity_std_dev()->set_z(gpchcx.vu_std); + // ins->mutable_euler_angles_std_dev()->set_x(gpchcx.roll_std * DEG_TO_RAD); + // // Convert std dev to rad + // ins->mutable_euler_angles_std_dev()->set_y(gpchcx.pitch_std * DEG_TO_RAD); + // // Convert std dev to rad + // ins->mutable_euler_angles_std_dev()->set_z(gpchcx.heading_std * + // DEG_TO_RAD); // Convert std dev to rad +} + +void HuaceParser::FillInsStat(const huace::Status& status, InsStat* ins_stat) { + ins_stat->set_ins_status(static_cast(status)); + + // IMPROVE: Map huace::Status to InsStat::GpsInfo and PosType + // This requires knowing the mapping from Huace status codes to these enums. + // Example (PLACEHOLDER - CHECK HUACE SPEC AND PROTO DEFINITIONS): + /* + switch(status) { + case huace::Status::RTK_STABLE_ORIENT: + ins_stat->set_gps_info(InsStat::GPS_FIX); // Or RTK_FIX? + ins_stat->set_pos_type(InsStat::INS_RTK_FIXED); + break; + case huace::Status::RTK_FLOAT_ORIENT: + ins_stat->set_gps_info(InsStat::GPS_FIX); // Or RTK_FLOAT? + ins_stat->set_pos_type(InsStat::INS_RTK_FLOAT); + break; + case huace::Status::SINGLE_POS_ORIENT: + case huace::Status::SINGLE_POS_NO_ORIENT: + ins_stat->set_gps_info(InsStat::GPS_FIX); // Or GPS_SINGLE? + ins_stat->set_pos_type(InsStat::INS_SINGLE); + break; + // ... handle other statuses ... + default: + ins_stat->set_gps_info(InsStat::NO_FIX); + ins_stat->set_pos_type(InsStat::INS_INVALID); + break; + } + */ + // If a direct mapping is not possible, logging the raw status code might be + // sufficient depending on requirements. +} + +} // namespace gnss +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/gnss/parser/huace/huace_parser.h b/modules/drivers/gnss/parser/huace/huace_parser.h new file mode 100644 index 00000000000..e8bc4544553 --- /dev/null +++ b/modules/drivers/gnss/parser/huace/huace_parser.h @@ -0,0 +1,129 @@ +// Copyright 2024 daohu527@gmail.com +// +// 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. + +// Created Date: 2024-12-30 +// Author: daohu527 + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "modules/drivers/gnss/proto/gnss_best_pose.pb.h" +#include "modules/drivers/gnss/proto/heading.pb.h" +#include "modules/drivers/gnss/proto/imu.pb.h" +#include "modules/drivers/gnss/proto/ins.pb.h" +#include "modules/drivers/gnss/proto/ins_stat.pb.h" + +#include "modules/drivers/gnss/parser/parser.h" + +namespace apollo { +namespace drivers { +namespace gnss { + +// Concrete parser for Huace GNSS receiver data (e.g., GPCHC, GPCHCX, GAPPA). +class HuaceParser : public Parser { + public: + // Specific frame types handled by this parser. + enum class FrameType { + UNKNOWN, // Unrecognized or initial state + GPCHC, // GPCHC frame type + GPCHCX, // GPCHCX frame type + GAPPA // GPGGA passthrough frame type + // Add other Huace frame types here + }; + + struct FieldParser { + const char* name; + std::function parse; + }; + + // Constructor (can take config if needed for Huace-specific settings like + // baud rate, etc.) explicit HuaceParser(const config::Config &config); // + // Example + HuaceParser() = default; + + // Virtual destructor as required by the base class. + ~HuaceParser() override = default; + + // Overridden methods from the base Parser class. + // See base class for detailed contract description. + bool ProcessHeader() override; + std::optional> ProcessPayload() override; + + private: + // Static map associating frame header strings with their corresponding + // FrameType. Defined in the .cpp file. Use string for easy comparison with + // buffer content. + static const std::unordered_map FRAME_HEADER_MAP; + + // Helper methods to parse specific frame types after a complete frame's data + // is extracted. These methods handle field splitting, type conversion, and + // internal error handling (logging, not throwing). They return a vector of + // parsed messages for that frame, or an empty vector if parsing failed + // internally. + std::vector ParseGPCHC(const std::vector& frame_data); + std::vector ParseGPCHCX( + const std::vector& frame_data); + std::vector ParseGAPPA(const std::vector& frame_data); + + // Helper method to parse common fields present in multiple GPCHCX-like + // frames. Takes a vector of string fields and populates a GPCHCX struct. + // Handles missing fields and invalid conversions internally via logging and + // default values (not throwing). + void ParseCommonGPCHCXFields(const std::vector& items, + GPCHCX& gpchcx); + + // Helper method to validate the checksum of a complete frame. + // Assumes the checksum type and location are defined in huace_protocol.h + // (e.g., CRC32 at the end). + bool IsChecksumValid(const std::vector& frame_data); + + // Helper method to extract and split the payload section of a text frame into + // fields. Assumes comma delimiter and knows how to exclude + // checksum/terminator parts. Returns a vector of strings representing the + // fields. + std::vector ExtractAndSplitPayload( + const std::vector& frame_data); + + // Helper methods to fill Protobuf messages from the parsed GPCHCX struct. + // These methods take a const reference to the parsed data and a pointer to + // the Protobuf message. + void FillGnssBestpos(const GPCHCX& gpchcx, GnssBestPose* bestpos); + void FillImu(const GPCHCX& gpchcx, Imu* imu); + void FillHeading(const GPCHCX& gpchcx, Heading* heading); + void FillIns(const GPCHCX& gpchcx, Ins* ins); + void FillInsStat(const huace::Status& status, InsStat* ins_stat); + + private: + // State variable storing the type of the frame currently being processed. + FrameType current_frame_type_ = FrameType::UNKNOWN; + + // State variable to store the size of the header that was consumed by + // ProcessHeader. Needed to correctly calculate total frame size after finding + // terminator in ProcessPayload. + size_t current_header_size_ = 0; +}; + +// Helper functions for status conversion (definitions in the .cpp file within +// an anonymous namespace) SolutionStatus ToSolutionStatus(huace::Status +// status); SolutionType ToSolutionType(huace::Status status); + +} // namespace gnss +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/gnss/parser/novatel/BUILD b/modules/drivers/gnss/parser/novatel/BUILD new file mode 100644 index 00000000000..c989ad2dc23 --- /dev/null +++ b/modules/drivers/gnss/parser/novatel/BUILD @@ -0,0 +1,31 @@ +load("@rules_cc//cc:defs.bzl", "cc_library") +load("//tools:cpplint.bzl", "cpplint") + +cc_library( + name = "novatel_parser", + srcs = ["novatel_parser.cc"], + hdrs = [ + "novatel_messages.h", + "parser.h", + "rtcm_decode.h", + ], + copts = ["-Ithird_party/rtklib"], + deps = [ + "//cyber", + "//modules/common/adapters:adapter_gflags", + "//modules/common_msgs/basic_msgs:error_code_cc_proto", + "//modules/common_msgs/basic_msgs:geometry_cc_proto", + "//modules/common_msgs/basic_msgs:header_cc_proto", + "//modules/drivers/gnss/proto:config_cc_proto", + "//modules/common_msgs/sensor_msgs:gnss_best_pose_cc_proto", + "//modules/common_msgs/sensor_msgs:gnss_cc_proto", + "//modules/common_msgs/sensor_msgs:gnss_raw_observation_cc_proto", + "//modules/common_msgs/sensor_msgs:heading_cc_proto", + "//modules/common_msgs/sensor_msgs:imu_cc_proto", + "//modules/common_msgs/sensor_msgs:ins_cc_proto", + "//modules/drivers/gnss/util:gnss_util", + "//third_party/rtklib", + ], +) + +cpplint() diff --git a/modules/drivers/gnss/parser/novatel_messages.h b/modules/drivers/gnss/parser/novatel/novatel_messages.h similarity index 99% rename from modules/drivers/gnss/parser/novatel_messages.h rename to modules/drivers/gnss/parser/novatel/novatel_messages.h index 26e1d7d6a78..f4952b2fbdc 100644 --- a/modules/drivers/gnss/parser/novatel_messages.h +++ b/modules/drivers/gnss/parser/novatel/novatel_messages.h @@ -25,7 +25,7 @@ #include #include -#include "modules/drivers/gnss/proto/config.pb.h" +#include "gnss/proto/config.pb.h" namespace apollo { namespace drivers { @@ -524,6 +524,7 @@ struct ImuParameter { }; using ::apollo::drivers::gnss::config::ImuType; + inline ImuParameter GetImuParameter(ImuType type) { switch (type) { case ImuType::IMAR_FSAS: diff --git a/modules/drivers/gnss/parser/novatel_parser.cc b/modules/drivers/gnss/parser/novatel/novatel_parser.cc similarity index 51% rename from modules/drivers/gnss/parser/novatel_parser.cc rename to modules/drivers/gnss/parser/novatel/novatel_parser.cc index bf20c8dd7c9..fb3fd7c88be 100644 --- a/modules/drivers/gnss/parser/novatel_parser.cc +++ b/modules/drivers/gnss/parser/novatel/novatel_parser.cc @@ -18,483 +18,584 @@ // messages must be // logged in order for this parser to work properly. // +#include "gnss/parser/novatel/novatel_parser.h" + #include #include #include #include -#include -#include "cyber/cyber.h" +#include "gnss/parser/rtcm_decode.h" +#include "gnss/util/time_conversion.h" -#include "modules/drivers/gnss/parser/novatel_messages.h" -#include "modules/drivers/gnss/parser/parser.h" -#include "modules/drivers/gnss/parser/rtcm_decode.h" -#include "modules/common_msgs/sensor_msgs/gnss.pb.h" -#include "modules/common_msgs/sensor_msgs/gnss_best_pose.pb.h" -#include "modules/common_msgs/sensor_msgs/gnss_raw_observation.pb.h" -#include "modules/common_msgs/sensor_msgs/heading.pb.h" -#include "modules/common_msgs/sensor_msgs/imu.pb.h" -#include "modules/common_msgs/sensor_msgs/ins.pb.h" -#include "modules/drivers/gnss/util/time_conversion.h" +#include "cyber/common/log.h" namespace apollo { namespace drivers { namespace gnss { -// Anonymous namespace that contains helper constants and functions. -namespace { - -constexpr size_t BUFFER_SIZE = 256; - -constexpr int SECONDS_PER_WEEK = 60 * 60 * 24 * 7; - -constexpr double DEG_TO_RAD = M_PI / 180.0; - -constexpr float FLOAT_NAN = std::numeric_limits::quiet_NaN(); - -// The NovAtel's orientation covariance matrix is pitch, roll, and yaw. We use -// the index array below -// to convert it to the orientation covariance matrix with order roll, pitch, -// and yaw. -constexpr int INDEX[] = {4, 3, 5, 1, 0, 2, 7, 6, 8}; -static_assert(sizeof(INDEX) == 9 * sizeof(int), "Incorrect size of INDEX"); +NovatelParser::NovatelParser() : Parser(4096) { + ins_.mutable_position_covariance()->Resize(9, kFloatNaN); + ins_.mutable_euler_angles_covariance()->Resize(9, kFloatNaN); + ins_.mutable_linear_velocity_covariance()->Resize(9, kFloatNaN); -template -constexpr bool is_zero(T value) { - return value == static_cast(0); -} - -// CRC algorithm from the NovAtel document. -inline uint32_t crc32_word(uint32_t word) { - for (int j = 0; j < 8; ++j) { - if (word & 1) { - word = (word >> 1) ^ 0xEDB88320; - } else { - word >>= 1; - } + if (1 != init_raw(&raw_)) { + AFATAL << "memory allocation error for observation data structure."; } - return word; } -inline uint32_t crc32_block(const uint8_t* buffer, size_t length) { - uint32_t word = 0; - while (length--) { - uint32_t t1 = (word >> 8) & 0xFFFFFF; - uint32_t t2 = crc32_word((word ^ *buffer++) & 0xFF); - word = t1 ^ t2; - } - return word; -} +NovatelParser::NovatelParser(const config::Config& config) + : Parser(config.data().buffer_size()) { + ins_.mutable_position_covariance()->Resize(9, kFloatNaN); + ins_.mutable_euler_angles_covariance()->Resize(9, kFloatNaN); + ins_.mutable_linear_velocity_covariance()->Resize(9, kFloatNaN); -// Converts NovAtel's azimuth (north = 0, east = 90) to FLU yaw (east = 0, north -// = pi/2). -constexpr double azimuth_deg_to_yaw_rad(double azimuth) { - return (90.0 - azimuth) * DEG_TO_RAD; -} + if (config.has_imu_type()) { + imu_type_ = config.imu_type(); + } -// A helper that fills an Point3D object (which uses the FLU frame) using RFU -// measurements. -inline void rfu_to_flu(double r, double f, double u, - ::apollo::common::Point3D* flu) { - flu->set_x(f); - flu->set_y(-r); - flu->set_z(u); + if (1 != init_raw(&raw_)) { + AFATAL << "memory allocation error for observation data structure."; + } } -} // namespace - -class NovatelParser : public Parser { - public: - NovatelParser(); - explicit NovatelParser(const config::Config& config); - - virtual MessageType GetMessage(MessagePtr* message_ptr); - - private: - bool check_crc(); - - Parser::MessageType PrepareMessage(MessagePtr* message_ptr); - - // The handle_xxx functions return whether a message is ready. - bool HandleBestPos(const novatel::BestPos* pos, uint16_t gps_week, - uint32_t gps_millisecs); - - bool HandleGnssBestpos(const novatel::BestPos* pos, uint16_t gps_week, - uint32_t gps_millisecs); - - bool HandleBestVel(const novatel::BestVel* vel, uint16_t gps_week, - uint32_t gps_millisecs); - - bool HandleCorrImuData(const novatel::CorrImuData* imu); - - bool HandleInsCov(const novatel::InsCov* cov); - - bool HandleInsPva(const novatel::InsPva* pva); - - bool HandleInsPvax(const novatel::InsPvaX* pvax, uint16_t gps_week, - uint32_t gps_millisecs); - - bool HandleRawImuX(const novatel::RawImuX* imu); - - bool HandleRawImu(const novatel::RawImu* imu); - - bool HandleBdsEph(const novatel::BDS_Ephemeris* bds_emph); - - bool HandleGpsEph(const novatel::GPS_Ephemeris* gps_emph); - - bool HandleGloEph(const novatel::GLO_Ephemeris* glo_emph); - - void SetObservationTime(); - - bool DecodeGnssObservation(const uint8_t* obs_data, - const uint8_t* obs_data_end); +bool NovatelParser::ProcessHeader() { + // --- State Restoration and Precondition Check --- + // If a partial header was stored from a previous call (header_bytes_ is not + // empty). This state is entered when sync bytes were found but there wasn't + // enough data for the full header. + if (!header_bytes_.empty()) { + // We already have partial header data stored in header_bytes_. + // Need to Read the remaining part from the DataBuffer. + // total_length_ at this point should store the expected full header length + // (set when sync was found but data was insufficient). + size_t needed = total_length_ - header_bytes_.size(); + if (buffer_.Size() < needed) { + AINFO_EVERY(500) + << "Continuing header read, but not enough data for remaining " + << needed << " bytes. Have " << buffer_.Size(); + return false; // Still not enough data + } - bool HandleHeading(const novatel::Heading* heading, uint16_t gps_week, - uint32_t gps_millisecs); - double gyro_scale_ = 0.0; + // Read the remaining header part. Read consumes. + auto rest_of_header_opt = buffer_.Read(needed); + if (!rest_of_header_opt || rest_of_header_opt->size() < needed) { + // Unexpected error, theoretically Read should not fail if Size check + // passed + AERROR << "Failed to read rest of header bytes (continue state, " + "unexpected). " + "Consuming remaining buffer."; + buffer_.Consume(buffer_.Size()); // Clear buffer to recover + header_bytes_.clear(); // Clear stored partial header + total_length_ = 0; // Clear expected full header length state + header_length_ = 0; + // Reset other potential search states, e.g., sync_window_ + sync_window_.clear(); + return false; // Error + } + const std::vector& rest_of_header = *rest_of_header_opt; + + // Append the Read data to the stored partial header to form the full header + // copy. + header_bytes_.insert(header_bytes_.end(), rest_of_header.begin(), + rest_of_header.end()); + + // The full header has been Read and stored in header_bytes_. + // Now calculate total message length and prepare to enter ProcessPayload + // state. + header_length_ = header_bytes_.size(); + + uint16_t message_length; + // Use data() for reinterpret_cast on the vector copy + if (header_length_ == sizeof(novatel::LongHeader)) { + message_length = + reinterpret_cast(header_bytes_.data()) + ->message_length; + } else { // ShortHeader + message_length = + reinterpret_cast(header_bytes_.data()) + ->message_length; + } - double accel_scale_ = 0.0; + // Calculate total message length (Header + Payload + CRC) + // total_length_ is now reset to the full message length + total_length_ = header_length_ + message_length + novatel::CRC_LENGTH; - float imu_measurement_span_ = 1.0f / 200.0f; - float imu_measurement_hz_ = 200.0f; + // header_bytes_ already contains the full header copy - int imu_frame_mapping_ = 5; + AINFO_EVERY(500) << "Finished reading header (continue state). Consumed " + << needed << " bytes from buffer. Total header len: " + << header_length_ << ", Message len: " << message_length + << ", Total len: " << total_length_; + // Header has been Read and stored, can return true to enter ProcessPayload + return true; + } - double imu_measurement_time_previous_ = -1.0; + // --- Search for Header from Start --- + // Use Poll() to read byte by byte and discard garbage until AA is found, then + // check for AA 44/45 12/13 sequence. Use a small sliding window to check sync + // bytes during Poll(). Assuming we have a member std::vector + // sync_window_ to maintain recent bytes. + const size_t SYNC_WINDOW_SIZE = 3; // AA XX YY - std::vector buffer_; + while (auto byte_opt = buffer_.Poll()) { // Poll consumes 1 byte + uint8_t byte = *byte_opt; - size_t header_length_ = 0; + // Add current byte to the sliding window + sync_window_.push_back(byte); + if (sync_window_.size() > SYNC_WINDOW_SIZE) { + sync_window_.erase(sync_window_.begin()); // Maintain window size + } - size_t total_length_ = 0; + // Check if window contains sync sequence (only check when window is full) + if (sync_window_.size() == SYNC_WINDOW_SIZE) { + if (sync_window_[0] == novatel::SYNC_0 && + (sync_window_[1] == novatel::SYNC_1 || + sync_window_[1] == novatel::SYNC_1_ALT) && + (sync_window_[2] == novatel::SYNC_2_LONG_HEADER || + sync_window_[2] == novatel::SYNC_2_SHORT_HEADER)) { + // **Sync sequence found!** These 3 bytes have already been consumed by + // Poll. + size_t current_header_len = + (sync_window_[2] == novatel::SYNC_2_LONG_HEADER) + ? sizeof(novatel::LongHeader) + : sizeof(novatel::ShortHeader); + + size_t remaining_header_len = current_header_len - SYNC_WINDOW_SIZE; + + // Need to check if there is enough remaining data in the buffer for the + // rest of the header. + if (buffer_.Size() < remaining_header_len) { + // Not enough data. Store the 3 sync bytes from the window and wait + // for more data. header_bytes_ is used here to store the partial + // header (sync bytes). total_length_ is used here to store the + // expected full header length. + header_bytes_.assign(sync_window_.begin(), sync_window_.end()); + total_length_ = + current_header_len; // Store expected full header length + header_length_ = 0; // Full header not obtained yet + sync_window_.clear(); // Clear window + AINFO_EVERY(500) + << "Found sync (AA 44/45 12/13), but buffer too small " + "for rest of header (need " + << remaining_header_len << ", have " << buffer_.Size() + << "). Storing partial header."; + return false; // Wait for more data + } - config::ImuType imu_type_ = config::ImuType::ADIS16488; + // **Enough data, Read the remaining header part. Read consumes.** + auto rest_of_header_opt = buffer_.Read(remaining_header_len); + if (!rest_of_header_opt || + rest_of_header_opt->size() < remaining_header_len) { + // Unexpected error, theoretically Read should not fail if Size check + // passed + AERROR << "Failed to read rest of header bytes (unexpected). " + "Consuming remaining buffer."; + buffer_.Consume(buffer_.Size()); // Clear buffer to recover + sync_window_.clear(); + header_bytes_.clear(); // Clear stored partial header + total_length_ = 0; // Clear expected full header length state + header_length_ = 0; + return false; // Error + } + const std::vector& rest_of_header = *rest_of_header_opt; + + // Combine the 3 sync bytes from the window (already consumed by Poll) + // with the remaining header part Read from the buffer, to form the full + // header copy. + header_bytes_.assign( + sync_window_.begin(), + sync_window_.end()); // Start with the 3 sync bytes + header_bytes_.insert(header_bytes_.end(), rest_of_header.begin(), + rest_of_header.end()); // Append the rest + + // The full header copy is now stored in header_bytes_. + // Now calculate total message length and prepare to enter + // ProcessPayload state. + header_length_ = header_bytes_.size(); + uint16_t message_length; + // Use data() for reinterpret_cast on the vector copy + if (header_length_ == sizeof(novatel::LongHeader)) { + message_length = + reinterpret_cast(header_bytes_.data()) + ->message_length; + } else { // ShortHeader + message_length = reinterpret_cast( + header_bytes_.data()) + ->message_length; + } - // -1 is an unused value. - novatel::SolutionStatus solution_status_ = - static_cast(novatel::SolutionStatus::NONE); - novatel::SolutionType position_type_ = - static_cast(novatel::SolutionType::NONE); - novatel::SolutionType velocity_type_ = - static_cast(novatel::SolutionType::NONE); - novatel::InsStatus ins_status_ = - static_cast(novatel::InsStatus::NONE); + // Calculate total message length (Header + Payload + CRC) + total_length_ = header_length_ + message_length + novatel::CRC_LENGTH; - raw_t raw_; // used for observation data + // Clear window as we found the header + sync_window_.clear(); + // total_length_ and header_length_ are already set to full message and + // header lengths header_bytes_ stores the full header copy - ::apollo::drivers::gnss::Gnss gnss_; - ::apollo::drivers::gnss::GnssBestPose bestpos_; - ::apollo::drivers::gnss::Imu imu_; - ::apollo::drivers::gnss::Ins ins_; - ::apollo::drivers::gnss::InsStat ins_stat_; - ::apollo::drivers::gnss::GnssEphemeris gnss_ephemeris_; - ::apollo::drivers::gnss::EpochObservation gnss_observation_; - ::apollo::drivers::gnss::Heading heading_; -}; + AINFO_EVERY(500) << "Found and read header. Consumed " + << SYNC_WINDOW_SIZE + remaining_header_len << " bytes." + << " Header len: " << header_length_ + << ", Message len: " << message_length + << ", Total len: " << total_length_ + << ". Remaining buffer size: " << buffer_.Size(); + return true; + } else { + // Bytes in the window do not form a sync sequence. Continue polling, + // window slides. No extra Consume needed, Poll already consumed 1 byte. + } + } + } -Parser* Parser::CreateNovatel(const config::Config& config) { - return new NovatelParser(config); + // Buffer is empty. If partial header state exists, it means waiting for + // remaining data, return false. If partial header state does not exist, it + // means sync sequence was not found, also return false. Therefore, just + // return false if the Buffer is empty. + AINFO_EVERY(500) << "Buffer empty while searching for header."; + return false; // Need more data } -NovatelParser::NovatelParser() { - buffer_.reserve(BUFFER_SIZE); - ins_.mutable_position_covariance()->Resize(9, FLOAT_NAN); - ins_.mutable_euler_angles_covariance()->Resize(9, FLOAT_NAN); - ins_.mutable_linear_velocity_covariance()->Resize(9, FLOAT_NAN); +std::optional> NovatelParser::ProcessPayload() { + // total_length_ and header_length_ are set by ProcessHeader (full lengths). + // header_bytes_ member stores the full header copy Read by ProcessHeader. - if (1 != init_raw(&raw_)) { - AFATAL << "memory allocation error for observation data structure."; - } -} - -NovatelParser::NovatelParser(const config::Config& config) { - buffer_.reserve(BUFFER_SIZE); - ins_.mutable_position_covariance()->Resize(9, FLOAT_NAN); - ins_.mutable_euler_angles_covariance()->Resize(9, FLOAT_NAN); - ins_.mutable_linear_velocity_covariance()->Resize(9, FLOAT_NAN); + size_t remaining_payload_crc_size = total_length_ - header_length_; - if (config.has_imu_type()) { - imu_type_ = config.imu_type(); + // Check if remaining data in the buffer is enough for the full payload + CRC + if (buffer_.Size() < remaining_payload_crc_size) { + AINFO_EVERY(500) << "Buffer too small for Novatel payload+CRC (need " + << remaining_payload_crc_size << ", have " + << buffer_.Size() << ")."; + return std::nullopt; // Not enough data } - if (1 != init_raw(&raw_)) { - AFATAL << "memory allocation error for observation data structure."; + // Read (copy) the payload + CRC part. Read consumes. + auto payload_crc_bytes_copy_opt = buffer_.Read(remaining_payload_crc_size); + if (!payload_crc_bytes_copy_opt || + payload_crc_bytes_copy_opt->size() < remaining_payload_crc_size) { + // Unexpected error, theoretically Read should not fail if Size check passed + AERROR << "Failed to read payload+CRC data from buffer (unexpected). " + "Consuming remaining buffer."; + buffer_.Consume(buffer_.Size()); // Clear buffer to recover + header_bytes_.clear(); // Clear stored header copy + total_length_ = 0; // Reset state + header_length_ = 0; + // Reset ProcessHeader search state and partial header state + sync_window_.clear(); + // stored_partial_header_.clear(); expected_full_header_length_ = 0; // If + // using separate members + return std::vector(); // Return empty vector signaling + // consumption and state switch to + // SEEK_HEADER (assuming this is the + // error handling mechanism) } -} - -Parser::MessageType NovatelParser::GetMessage(MessagePtr* message_ptr) { - if (data_ == nullptr) { - return MessageType::NONE; + const std::vector& payload_crc_bytes_copy = + *payload_crc_bytes_copy_opt; // Get copy + + // Combine stored header copy (header_bytes_) and the payload+CRC copy Read + // (payload_crc_bytes_copy), for CRC check + std::vector full_message_block; + full_message_block.reserve(total_length_); // Reserve space for total length + full_message_block.insert(full_message_block.end(), header_bytes_.begin(), + header_bytes_.end()); // Add stored header copy + full_message_block.insert( + full_message_block.end(), payload_crc_bytes_copy.begin(), + payload_crc_bytes_copy.end()); // Add payload+CRC copy + + // Perform CRC check on the combined continuous memory block + // CheckCRC function needs access to the entire message block + if (!CheckCRC(full_message_block.data(), full_message_block.size())) { + AERROR << "Novatel message CRC check failed. Total length: " + << total_length_; + // Payload+CRC has already been consumed by Read. The header was consumed in + // ProcessHeader. Only need to clear state variables. + header_bytes_.clear(); // Clear stored header copy + total_length_ = 0; // Reset state + header_length_ = 0; + // Reset ProcessHeader search state and partial header state + sync_window_.clear(); + // stored_partial_header_.clear(); expected_full_header_length_ = 0; // If + // using separate members + return std::vector(); } - while (data_ < data_end_) { - if (buffer_.empty()) { // Looking for SYNC0 - if (*data_ == novatel::SYNC_0) { - buffer_.push_back(*data_); - } - ++data_; - } else if (buffer_.size() == 1) { // Looking for SYNC1 - if (*data_ == novatel::SYNC_1) { - buffer_.push_back(*data_++); - } else { - buffer_.clear(); - } - } else if (buffer_.size() == 2) { // Looking for SYNC2 - switch (*data_) { - case novatel::SYNC_2_LONG_HEADER: - buffer_.push_back(*data_++); - header_length_ = sizeof(novatel::LongHeader); - break; - case novatel::SYNC_2_SHORT_HEADER: - buffer_.push_back(*data_++); - header_length_ = sizeof(novatel::ShortHeader); - break; - default: - buffer_.clear(); - } - } else if (header_length_ > 0) { // Working on header. - if (buffer_.size() < header_length_) { - buffer_.push_back(*data_++); - } else { - if (header_length_ == sizeof(novatel::LongHeader)) { - total_length_ = header_length_ + novatel::CRC_LENGTH + - reinterpret_cast(buffer_.data()) - ->message_length; - } else if (header_length_ == sizeof(novatel::ShortHeader)) { - total_length_ = - header_length_ + novatel::CRC_LENGTH + - reinterpret_cast(buffer_.data()) - ->message_length; - } else { - AERROR << "Incorrect header_length_. Should never reach here."; - buffer_.clear(); - } - header_length_ = 0; - } - } else if (total_length_ > 0) { - if (buffer_.size() < total_length_) { // Working on body. - buffer_.push_back(*data_++); - continue; - } - MessageType type = PrepareMessage(message_ptr); - buffer_.clear(); - total_length_ = 0; - if (type != MessageType::NONE) { - return type; - } - } - } - return MessageType::NONE; + // CRC check passed, process payload using the payload+CRC copy Read + const uint8_t* payload_data = payload_crc_bytes_copy.data(); + size_t payload_size = remaining_payload_crc_size - novatel::CRC_LENGTH; + + // Call PrepareMessage (needs payload data and header data) + // PrepareMessage receives payload_data (payload part from copied payload+CRC) + // and header_bytes_.data() (stored header copy) + std::vector parsed_messages = + PrepareMessage(payload_data, payload_size, header_bytes_.data()); + + // Payload + CRC has already been consumed by Read, no need to consume again. + // buffer_.Consume(remaining_payload_crc_size); // This line was not needed + + // Clear stored header, reset length state variables, prepare for the next + // message + header_bytes_.clear(); + total_length_ = 0; + header_length_ = 0; + // Reset ProcessHeader search state and partial header state + sync_window_.clear(); + // stored_partial_header_.clear(); expected_full_header_length_ = 0; // If + // using separate members + + return parsed_messages; } -bool NovatelParser::check_crc() { - size_t l = buffer_.size() - novatel::CRC_LENGTH; - return crc32_block(buffer_.data(), l) == - *reinterpret_cast(buffer_.data() + l); +// Adapted CheckCRC - Takes data pointer and size for the *full* message block +// (header + payload + CRC) +bool NovatelParser::CheckCRC(const uint8_t* data, size_t size) { + if (size < novatel::CRC_LENGTH) { + return false; + } + size_t message_and_header_len = size - novatel::CRC_LENGTH; + const uint32_t expected_crc = + *reinterpret_cast(data + message_and_header_len); + const uint32_t actual_crc = crc32_block(data, message_and_header_len); + return actual_crc == expected_crc; } -Parser::MessageType NovatelParser::PrepareMessage(MessagePtr* message_ptr) { - if (!check_crc()) { - AERROR << "CRC check failed."; - return MessageType::NONE; - } +std::vector NovatelParser::PrepareMessage( + const uint8_t* payload_data, size_t payload_size, + const uint8_t* header_data) { + std::vector messages; - uint8_t* message = nullptr; + // Extract header info novatel::MessageId message_id; - uint16_t message_length; uint16_t gps_week; uint32_t gps_millisecs; - if (buffer_[2] == novatel::SYNC_2_LONG_HEADER) { - auto header = reinterpret_cast(buffer_.data()); - message = buffer_.data() + sizeof(novatel::LongHeader); + // Assuming header_data size is header_length_ + if (header_length_ == sizeof(novatel::LongHeader)) { + const auto* header = + reinterpret_cast(header_data); gps_week = header->gps_week; gps_millisecs = header->gps_millisecs; message_id = header->message_id; - message_length = header->message_length; - } else { - auto header = reinterpret_cast(buffer_.data()); - message = buffer_.data() + sizeof(novatel::ShortHeader); + } else if (header_length_ == sizeof(novatel::ShortHeader)) { + const auto* header = + reinterpret_cast(header_data); gps_week = header->gps_week; - gps_millisecs = header->gps_millisecs; + gps_millisecs = + header->gps_millisecs; // ShortHeader might not have full time info? + // Check Novatel docs. Assuming it does for now. message_id = header->message_id; - message_length = header->message_length; + } else { + AERROR << "PrepareMessage called with invalid header_length_: " + << header_length_; + return messages; // Return empty } + switch (message_id) { case novatel::BESTGNSSPOS: - if (message_length != sizeof(novatel::BestPos)) { - AERROR << "Incorrect message_length"; + if (payload_size != sizeof(novatel::BestPos)) { /* log error */ break; } - if (HandleGnssBestpos(reinterpret_cast(message), - gps_week, gps_millisecs)) { - *message_ptr = &bestpos_; - return MessageType::BEST_GNSS_POS; + if (HandleGnssBestpos( + reinterpret_cast(payload_data), gps_week, + gps_millisecs, &bestpos_)) { + auto msg_ptr = std::make_unique(); + msg_ptr->CopyFrom(bestpos_); + messages.emplace_back(MessageType::BEST_GNSS_POS, std::move(msg_ptr)); } break; case novatel::BESTPOS: case novatel::PSRPOS: - if (message_length != sizeof(novatel::BestPos)) { - AERROR << "Incorrect message_length"; + if (payload_size != sizeof(novatel::BestPos)) { /* log error */ break; } - if (HandleBestPos(reinterpret_cast(message), gps_week, - gps_millisecs)) { - *message_ptr = &gnss_; - return MessageType::GNSS; + if (HandleBestPos(reinterpret_cast(payload_data), + gps_week, gps_millisecs, &gnss_)) { + // Gnss message might be completed by BESTVEL, we return it here based + // on original logic, potentially incomplete or combined later if + // BESTVEL follows closely. A better design might wait for both, but + // adhering to original handler output implies returning now. + auto msg_ptr = std::make_unique(); + msg_ptr->CopyFrom(gnss_); + messages.emplace_back(MessageType::GNSS, std::move(msg_ptr)); } break; case novatel::BESTGNSSVEL: case novatel::BESTVEL: case novatel::PSRVEL: - if (message_length != sizeof(novatel::BestVel)) { - AERROR << "Incorrect message_length"; + if (payload_size != sizeof(novatel::BestVel)) { /* log error */ break; } - if (HandleBestVel(reinterpret_cast(message), gps_week, - gps_millisecs)) { - *message_ptr = &gnss_; - return MessageType::GNSS; + // HandleBestVel updates internal gnss_. It might rely on pos data already + // being there. Original logic returned true, and message_ptr = &gnss_. + // Here we call handler and then create GNSS message if handler succeeds. + if (HandleBestVel(reinterpret_cast(payload_data), + gps_week, gps_millisecs, &gnss_)) { + auto msg_ptr = std::make_unique(); + msg_ptr->CopyFrom(gnss_); + messages.emplace_back(MessageType::GNSS, std::move(msg_ptr)); } break; - case novatel::CORRIMUDATA: + case novatel::CORRIMUDATA: // ... and other IMU/INS correction messages case novatel::CORRIMUDATAS: case novatel::IMURATECORRIMUS: - if (message_length != sizeof(novatel::CorrImuData)) { - AERROR << "Incorrect message_length"; + if (payload_size != sizeof(novatel::CorrImuData)) { /* log error */ break; } - - if (HandleCorrImuData(reinterpret_cast(message))) { - *message_ptr = &ins_; - return MessageType::INS; + // HandleCorrImuData updates internal ins_. + if (HandleCorrImuData( + reinterpret_cast(payload_data), + &ins_)) { + auto msg_ptr = std::make_unique(); + msg_ptr->CopyFrom(ins_); + messages.emplace_back(MessageType::INS, std::move(msg_ptr)); } break; - case novatel::INSCOV: + case novatel::INSCOV: // ... and INS covariance messages case novatel::INSCOVS: - if (message_length != sizeof(novatel::InsCov)) { - AERROR << "Incorrect message_length"; + if (payload_size != sizeof(novatel::InsCov)) { /* log error */ break; } - - if (HandleInsCov(reinterpret_cast(message))) { - *message_ptr = &ins_; - return MessageType::INS; - } + // HandleInsCov updates internal ins_. It returns false in original code, + // indicating covariance message itself doesn't generate a new INS + // message. + HandleInsCov(reinterpret_cast(payload_data), + &ins_); + // No message added to 'messages' for INSCOV alone based on original + // logic. break; - case novatel::INSPVA: + case novatel::INSPVA: // and other INS position/velocity/attitude messages case novatel::INSPVAS: - if (message_length != sizeof(novatel::InsPva)) { - AERROR << "Incorrect message_length"; + if (payload_size != sizeof(novatel::InsPva)) { /* log error */ break; } - - if (HandleInsPva(reinterpret_cast(message))) { - *message_ptr = &ins_; - return MessageType::INS; + // HandleInsPva updates internal ins_. + if (HandleInsPva(reinterpret_cast(payload_data), + &ins_)) { + auto msg_ptr = std::make_unique(); + msg_ptr->CopyFrom(ins_); + messages.emplace_back(MessageType::INS, std::move(msg_ptr)); } break; - case novatel::RAWIMUX: + case novatel::RAWIMUX: // ... and other Raw IMU messages case novatel::RAWIMUSX: - if (message_length != sizeof(novatel::RawImuX)) { - AERROR << "Incorrect message_length"; + if (payload_size != sizeof(novatel::RawImuX)) { /* log error */ break; } - - if (HandleRawImuX(reinterpret_cast(message))) { - *message_ptr = &imu_; - return MessageType::IMU; + // HandleRawImuX updates internal imu_. + if (HandleRawImuX(reinterpret_cast(payload_data), + &imu_)) { + auto msg_ptr = std::make_unique(); + msg_ptr->CopyFrom(imu_); + messages.emplace_back(MessageType::IMU, std::move(msg_ptr)); } break; case novatel::RAWIMU: case novatel::RAWIMUS: - if (message_length != sizeof(novatel::RawImu)) { - AERROR << "Incorrect message_length"; + if (payload_size != sizeof(novatel::RawImu)) { /* log error */ break; } - - if (HandleRawImu(reinterpret_cast(message))) { - *message_ptr = &imu_; - return MessageType::IMU; + // HandleRawImu updates internal imu_. + if (HandleRawImu(reinterpret_cast(payload_data), + &imu_)) { + auto msg_ptr = std::make_unique(); + msg_ptr->CopyFrom(imu_); + messages.emplace_back(MessageType::IMU, std::move(msg_ptr)); } break; - case novatel::INSPVAX: - if (message_length != sizeof(novatel::InsPvaX)) { - AERROR << "Incorrect message_length"; + case novatel::INSPVAX: // INS PVA with extended status + if (payload_size != sizeof(novatel::InsPvaX)) { /* log error */ break; } - - if (HandleInsPvax(reinterpret_cast(message), gps_week, - gps_millisecs)) { - *message_ptr = &ins_stat_; - return MessageType::INS_STAT; + // HandleInsPvax updates internal ins_stat_. + if (HandleInsPvax(reinterpret_cast(payload_data), + gps_week, gps_millisecs, &ins_stat_)) { + auto msg_ptr = std::make_unique(); + msg_ptr->CopyFrom(ins_stat_); + messages.emplace_back(MessageType::INS_STAT, std::move(msg_ptr)); } break; - case novatel::BDSEPHEMERIS: - if (message_length != sizeof(novatel::BDS_Ephemeris)) { - AERROR << "Incorrect BDSEPHEMERIS message_length"; + case novatel::BDSEPHEMERIS: // ... and other Ephemeris messages + if (payload_size != sizeof(novatel::BDS_Ephemeris)) { /* log error */ break; } - if (HandleBdsEph(reinterpret_cast(message))) { - *message_ptr = &gnss_ephemeris_; - return MessageType::BDSEPHEMERIDES; + // HandleBdsEph updates internal gnss_ephemeris_. + if (HandleBdsEph( + reinterpret_cast(payload_data), + &gnss_ephemeris_)) { + auto msg_ptr = std::make_unique(); + msg_ptr->CopyFrom(gnss_ephemeris_); + messages.emplace_back(MessageType::BDSEPHEMERIDES, std::move(msg_ptr)); } break; case novatel::GPSEPHEMERIS: - if (message_length != sizeof(novatel::GPS_Ephemeris)) { - AERROR << "Incorrect GPSEPHEMERIS message_length"; + if (payload_size != sizeof(novatel::GPS_Ephemeris)) { /* log error */ break; } - if (HandleGpsEph(reinterpret_cast(message))) { - *message_ptr = &gnss_ephemeris_; - return MessageType::GPSEPHEMERIDES; + // HandleGpsEph updates internal gnss_ephemeris_. + if (HandleGpsEph( + reinterpret_cast(payload_data), + &gnss_ephemeris_)) { + auto msg_ptr = std::make_unique(); + msg_ptr->CopyFrom(gnss_ephemeris_); + messages.emplace_back(MessageType::GPSEPHEMERIDES, std::move(msg_ptr)); } break; case novatel::GLOEPHEMERIS: - if (message_length != sizeof(novatel::GLO_Ephemeris)) { - AERROR << "Incorrect GLOEPHEMERIS message length"; + if (payload_size != sizeof(novatel::GLO_Ephemeris)) { /* log error */ break; } - if (HandleGloEph(reinterpret_cast(message))) { - *message_ptr = &gnss_ephemeris_; - return MessageType::GLOEPHEMERIDES; + // HandleGloEph updates internal gnss_ephemeris_. + if (HandleGloEph( + reinterpret_cast(payload_data), + &gnss_ephemeris_)) { + auto msg_ptr = std::make_unique(); + msg_ptr->CopyFrom(gnss_ephemeris_); + messages.emplace_back(MessageType::GLOEPHEMERIDES, std::move(msg_ptr)); } break; - case novatel::RANGE: - if (DecodeGnssObservation(buffer_.data(), - buffer_.data() + buffer_.size())) { - *message_ptr = &gnss_observation_; - return MessageType::OBSERVATION; + case novatel::RANGE: // Raw Observation Data + // DecodeGnssObservation handles the raw data block using input_oem4 + if (DecodeGnssObservation(payload_data, payload_size, + &gnss_observation_)) { + auto msg_ptr = + std::make_unique(); + msg_ptr->CopyFrom(gnss_observation_); + messages.emplace_back(MessageType::OBSERVATION, std::move(msg_ptr)); } break; - case novatel::HEADING: - if (message_length != sizeof(novatel::Heading)) { - AERROR << "Incorrect message_length"; + case novatel::HEADING: // Heading message + if (payload_size != sizeof(novatel::Heading)) { /* log error */ break; } - if (HandleHeading(reinterpret_cast(message), gps_week, - gps_millisecs)) { - *message_ptr = &heading_; - return MessageType::HEADING; + // HandleHeading updates internal heading_. + if (HandleHeading(reinterpret_cast(payload_data), + gps_week, gps_millisecs, &heading_)) { + auto msg_ptr = std::make_unique(); + msg_ptr->CopyFrom(heading_); + messages.emplace_back(MessageType::HEADING, std::move(msg_ptr)); } break; default: + AINFO_EVERY(100) << "Unknown Novatel message ID: " + << static_cast(message_id) + << ". Payload size: " << payload_size; + // Unknown message type, do nothing, return empty vector break; } - return MessageType::NONE; + + return messages; } bool NovatelParser::HandleGnssBestpos(const novatel::BestPos* pos, @@ -524,9 +625,9 @@ bool NovatelParser::HandleGnssBestpos(const novatel::BestPos* pos, bestpos_.set_galileo_beidou_used_mask(pos->galileo_beidou_used_mask); bestpos_.set_gps_glonass_used_mask(pos->gps_glonass_used_mask); - double seconds = gps_week * SECONDS_PER_WEEK + gps_millisecs * 1e-3; + double seconds = gps_week * kSecondsPerWeek + gps_millisecs * 1e-3; bestpos_.set_measurement_time(seconds); - // AINFO << "Best gnss pose:\r\n" << bestpos_.DebugString(); + return true; } @@ -588,7 +689,7 @@ bool NovatelParser::HandleBestPos(const novatel::BestPos* pos, case novatel::SolutionType::INS_PPP: gnss_.set_type(apollo::drivers::gnss::Gnss::PPP); break; - case novatel::SolutionType::PROPOGATED: + case novatel::SolutionType::PROPAGATED: gnss_.set_type(apollo::drivers::gnss::Gnss::PROPAGATED); break; default: @@ -603,10 +704,10 @@ bool NovatelParser::HandleBestPos(const novatel::BestPos* pos, << static_cast(pos->datum_id); } - double seconds = gps_week * SECONDS_PER_WEEK + gps_millisecs * 1e-3; + double seconds = gps_week * kSecondsPerWeek + gps_millisecs * 1e-3; if (gnss_.measurement_time() != seconds) { gnss_.set_measurement_time(seconds); - return false; + return true; } return true; } @@ -627,7 +728,7 @@ bool NovatelParser::HandleBestVel(const novatel::BestVel* vel, gnss_.mutable_linear_velocity()->set_y(vel->horizontal_speed * sin(yaw)); gnss_.mutable_linear_velocity()->set_z(vel->vertical_speed); - double seconds = gps_week * SECONDS_PER_WEEK + gps_millisecs * 1e-3; + double seconds = gps_week * kSecondsPerWeek + gps_millisecs * 1e-3; if (gnss_.measurement_time() != seconds) { gnss_.set_measurement_time(seconds); return false; @@ -645,7 +746,7 @@ bool NovatelParser::HandleCorrImuData(const novatel::CorrImuData* imu) { imu->z_angle_change * imu_measurement_hz_, ins_.mutable_angular_velocity()); - double seconds = imu->gps_week * SECONDS_PER_WEEK + imu->gps_seconds; + double seconds = imu->gps_week * kSecondsPerWeek + imu->gps_seconds; if (ins_.measurement_time() != seconds) { ins_.set_measurement_time(seconds); return false; @@ -660,7 +761,7 @@ bool NovatelParser::HandleInsCov(const novatel::InsCov* cov) { ins_.set_position_covariance( i, static_cast(cov->position_covariance[i])); ins_.set_euler_angles_covariance( - INDEX[i], static_cast((DEG_TO_RAD * DEG_TO_RAD) * + INDEX[i], static_cast((kDegToRad * kDegToRad) * cov->attitude_covariance[i])); ins_.set_linear_velocity_covariance( i, static_cast(cov->velocity_covariance[i])); @@ -676,8 +777,8 @@ bool NovatelParser::HandleInsPva(const novatel::InsPva* pva) { ins_.mutable_position()->set_lon(pva->longitude); ins_.mutable_position()->set_lat(pva->latitude); ins_.mutable_position()->set_height(pva->height); - ins_.mutable_euler_angles()->set_x(pva->roll * DEG_TO_RAD); - ins_.mutable_euler_angles()->set_y(-pva->pitch * DEG_TO_RAD); + ins_.mutable_euler_angles()->set_x(pva->roll * kDegToRad); + ins_.mutable_euler_angles()->set_y(-pva->pitch * kDegToRad); ins_.mutable_euler_angles()->set_z(azimuth_deg_to_yaw_rad(pva->azimuth)); ins_.mutable_linear_velocity()->set_x(pva->east_velocity); ins_.mutable_linear_velocity()->set_y(pva->north_velocity); @@ -697,7 +798,7 @@ bool NovatelParser::HandleInsPva(const novatel::InsPva* pva) { ins_.set_type(apollo::drivers::gnss::Ins::INVALID); } - double seconds = pva->gps_week * SECONDS_PER_WEEK + pva->gps_seconds; + double seconds = pva->gps_week * kSecondsPerWeek + pva->gps_seconds; if (ins_.measurement_time() != seconds) { ins_.set_measurement_time(seconds); return false; @@ -709,7 +810,7 @@ bool NovatelParser::HandleInsPva(const novatel::InsPva* pva) { bool NovatelParser::HandleInsPvax(const novatel::InsPvaX* pvax, uint16_t gps_week, uint32_t gps_millisecs) { - double seconds = gps_week * SECONDS_PER_WEEK + gps_millisecs * 1e-3; + double seconds = gps_week * kSecondsPerWeek + gps_millisecs * 1e-3; double unix_sec = apollo::drivers::util::gps2unix(seconds); ins_stat_.mutable_header()->set_timestamp_sec(unix_sec); ins_stat_.set_ins_status(pvax->ins_status); @@ -742,7 +843,7 @@ bool NovatelParser::HandleRawImuX(const novatel::RawImuX* imu) { imu_.set_measurement_span(imu_measurement_span_); } - double time = imu->gps_week * SECONDS_PER_WEEK + imu->gps_seconds; + double time = imu->gps_week * kSecondsPerWeek + imu->gps_seconds; if (imu_measurement_time_previous_ > 0.0 && fabs(time - imu_measurement_time_previous_ - imu_measurement_span_) > 1e-4) { @@ -802,7 +903,7 @@ bool NovatelParser::HandleRawImu(const novatel::RawImu* imu) { imu_.set_measurement_span(imu_measurement_span); } - double time = imu->gps_week * SECONDS_PER_WEEK + imu->gps_seconds; + double time = imu->gps_week * kSecondsPerWeek + imu->gps_seconds; if (imu_measurement_time_previous_ > 0.0 && fabs(time - imu_measurement_time_previous_ - imu_measurement_span) > 1e-4) { @@ -969,14 +1070,14 @@ bool NovatelParser::HandleHeading(const novatel::Heading* heading, heading_.set_pitch_std_dev(heading->pitch_std_dev); heading_.set_station_id(heading->station_id); heading_.set_satellite_tracked_number(heading->num_sats_tracked); - heading_.set_satellite_soulution_number(heading->num_sats_in_solution); + heading_.set_satellite_solution_number(heading->num_sats_in_solution); heading_.set_satellite_number_obs(heading->num_sats_ele); heading_.set_satellite_number_multi(heading->num_sats_l2); heading_.set_solution_source(heading->solution_source); heading_.set_extended_solution_status(heading->extended_solution_status); heading_.set_galileo_beidou_sig_mask(heading->galileo_beidou_sig_mask); heading_.set_gps_glonass_sig_mask(heading->gps_glonass_sig_mask); - double seconds = gps_week * SECONDS_PER_WEEK + gps_millisecs * 1e-3; + double seconds = gps_week * kSecondsPerWeek + gps_millisecs * 1e-3; heading_.set_measurement_time(seconds); return true; } @@ -1030,7 +1131,7 @@ bool NovatelParser::DecodeGnssObservation(const uint8_t* obs_data, } auto band_obs = sat_obs->add_band_obs(); - if (raw_.obs.data[i].code[i] == CODE_L1C) { + if (raw_.obs.data[i].code[j] == CODE_L1C) { band_obs->set_pseudo_type( apollo::drivers::gnss::PseudoType::CORSE_CODE); } else if (raw_.obs.data[i].code[i] == CODE_L1P) { diff --git a/modules/drivers/gnss/parser/novatel/novatel_parser.h b/modules/drivers/gnss/parser/novatel/novatel_parser.h new file mode 100644 index 00000000000..c11a9b9019b --- /dev/null +++ b/modules/drivers/gnss/parser/novatel/novatel_parser.h @@ -0,0 +1,168 @@ +// Copyright 2024 daohu527@gmail.com +// +// 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. + +// Created Date: 2024-12-30 +// Author: daohu527 + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "modules/drivers/gnss/proto/config.pb.h" +#include "modules/drivers/gnss/proto/gnss.pb.h" +#include "modules/drivers/gnss/proto/gnss_best_pose.pb.h" +#include "modules/drivers/gnss/proto/gnss_raw_observation.pb.h" +#include "modules/drivers/gnss/proto/heading.pb.h" +#include "modules/drivers/gnss/proto/imu.pb.h" +#include "modules/drivers/gnss/proto/ins.pb.h" +#include "modules/drivers/gnss/proto/ins_stat.pb.h" + +#include "modules/drivers/gnss/parser/novatel/novatel_messages.h" +#include "modules/drivers/gnss/parser/parser.h" + +namespace apollo { +namespace drivers { +namespace gnss { + +class NovatelParser : public Parser { + public: + NovatelParser(); + explicit NovatelParser(const config::Config& config); + ~NovatelParser() = default; + + // Override base class pure virtual methods for state machine logic + bool ProcessHeader() override; + std::optional> ProcessPayload() override; + + private: + // Helper function to check CRC - takes data pointer and size for the *full* + // message block + bool CheckCRC(const uint8_t* data, size_t size); + + // Helper function to dispatch message parsing based on ID + // Takes payload data/size and header data, populates internal members, and + // returns messages. + std::vector PrepareMessage(const uint8_t* payload_data, + size_t payload_size, + const uint8_t* header_data); + + // The handle_xxx functions - Now take a pointer to the target Protobuf object + // to populate. They return bool indicating successful data + // extraction/validation for this specific message type. + bool HandleBestPos(const novatel::BestPos* pos, uint16_t gps_week, + uint32_t gps_millisecs, + apollo::drivers::gnss::Gnss* gnss_msg); + + // Note: MessageType::BEST_GNSS_POS corresponds to Novatel BESTGNSSPOS, mapped + // to apollo::drivers::gnss::GnssBestPose + bool HandleGnssBestpos( + const novatel::BestPos* pos, uint16_t gps_week, uint32_t gps_millisecs, + apollo::drivers::gnss::GnssBestPose* bestpos_msg); // Changed type + + bool HandleBestVel(const novatel::BestVel* vel, uint16_t gps_week, + uint32_t gps_millisecs, + apollo::drivers::gnss::Gnss* gnss_msg); + + bool HandleCorrImuData(const novatel::CorrImuData* imu, + apollo::drivers::gnss::Ins* ins_msg); + + // Handles INS covariance, updates the INS message member + bool HandleInsCov(const novatel::InsCov* cov, + apollo::drivers::gnss::Ins* ins_msg); + + bool HandleInsPva(const novatel::InsPva* pva, + apollo::drivers::gnss::Ins* ins_msg); + + // Handles INS PVA with extended status, populates InsStat message member + bool HandleInsPvax( + const novatel::InsPvaX* pvax, uint16_t gps_week, uint32_t gps_millisecs, + apollo::drivers::gnss::InsStat* ins_stat_msg); // Changed type + + // Handles Raw IMU data (IMU measurements/integrals) + bool HandleRawImuX(const novatel::RawImuX* imu, + apollo::drivers::gnss::Imu* imu_msg); + bool HandleRawImu(const novatel::RawImu* imu, + apollo::drivers::gnss::Imu* imu_msg); + + // Handles Ephemeris data + bool HandleBdsEph(const novatel::BDS_Ephemeris* bds_emph, + apollo::drivers::gnss::GnssEphemeris* eph_msg); + bool HandleGpsEph(const novatel::GPS_Ephemeris* gps_emph, + apollo::drivers::gnss::GnssEphemeris* eph_msg); + bool HandleGloEph(const novatel::GLO_Ephemeris* glo_emph, + apollo::drivers::gnss::GnssEphemeris* eph_msg); + + // Helper for setting observation time based on RTKLIB structure + void SetObservationTime(apollo::drivers::gnss::GnssObservation* obs_msg); + + // Decodes raw observation data using RTKLIB functionality + // Takes data pointer and size of the observation payload, and target message + // pointer + bool DecodeGnssObservation(const uint8_t* obs_data, size_t size, + apollo::drivers::gnss::GnssObservation* obs_msg); + + // Handles Heading message + bool HandleHeading(const novatel::Heading* heading, uint16_t gps_week, + uint32_t gps_millisecs, + apollo::drivers::gnss::Heading* heading_msg); + + private: + // Temporary buffer to store header bytes between ProcessHeader and + // ProcessPayload + std::vector header_bytes_; + + // Store total message length and header length determined in ProcessHeader + size_t total_length_ = 0; + size_t header_length_ = 0; + + // IMU configuration and state + config::ImuType imu_type_ = config::ImuType::IMU_UNKNOWN; + double gyro_scale_ = 0.0; + double accel_scale_ = 0.0; + float imu_measurement_span_ = 0.0f; // Initialized on first IMU message + float imu_measurement_hz_ = 0.0f; // Initialized on first IMU message + int imu_frame_mapping_ = 5; // Default frame mapping (RFU to FLU) + double imu_measurement_time_previous_ = -1.0; + + // GNSS/INS status/type state variables (used for logging changes) + novatel::SolutionStatus solution_status_ = novatel::SolutionStatus::NONE; + novatel::SolutionType position_type_ = novatel::SolutionType::NONE; + novatel::SolutionType velocity_type_ = novatel::SolutionType::NONE; + novatel::InsStatus ins_status_ = novatel::InsStatus::NONE; + + // RTKLIB raw observation data structure + raw_t raw_; + + ::apollo::drivers::gnss::Gnss gnss_; // For combined position/velocity + ::apollo::drivers::gnss::GnssBestPose bestpos_; + ::apollo::drivers::gnss::Imu imu_; // For RAWIMU/RAWIMUX + ::apollo::drivers::gnss::Ins ins_; // For CORRIMUDATA, INSPVA, INSCOV + ::apollo::drivers::gnss::InsStat ins_stat_; // For INSPVAX + ::apollo::drivers::gnss::GnssEphemeris gnss_ephemeris_; // For Ephemerides + ::apollo::drivers::gnss::GnssObservation gnss_observation_; + ::apollo::drivers::gnss::Heading heading_; // For HEADING + + DISALLOW_COPY_AND_ASSIGN(NovatelParser); +}; + +} // namespace gnss +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/gnss/parser/parser.h b/modules/drivers/gnss/parser/parser.h index 7fc1b567589..24a04084b78 100644 --- a/modules/drivers/gnss/parser/parser.h +++ b/modules/drivers/gnss/parser/parser.h @@ -16,62 +16,38 @@ #pragma once -#include +#include +#include +#include #include +#include +#include +#include +#include -#include "google/protobuf/message.h" +#include "gnss/proto/config.pb.h" -#include "modules/drivers/gnss/proto/config.pb.h" - -#include "modules/drivers/gnss/util/macros.h" +#include "cyber/common/log.h" +#include "cyber/common/macros.h" +#include "modules/drivers/gnss/parser/huace/huace_parser.h" +#include "modules/drivers/gnss/parser/novatel/novatel_parser.h" +#include "modules/drivers/gnss/util/data_buffer.h" namespace apollo { namespace drivers { namespace gnss { -// convert gps time (base on Jan 6 1980) to system time (base on Jan 1 1970) -// notice: Jan 6 1980 -// -// linux shell: -// time1 = date +%s -d"Jan 6, 1980 00:00:01" -// time2 = date +%s -d"Jan 1, 1970 00:00:01" -// dif_tick = time1-time2 -// 315964800 = 315993601 - 28801 - -#define EPOCH_AND_SYSTEM_DIFF_SECONDS 315964800 - -// A helper function that returns a pointer to a protobuf message of type T. -template -inline T *As(::google::protobuf::Message *message_ptr) { - return dynamic_cast(message_ptr); -} - -// An abstract class of Parser. -// One should use the create_xxx() functions to create a Parser object. +// An abstract class of Parser for GNSS data streams. +// Derived classes implement parsing logic for specific data formats. class Parser { public: - // A general pointer to a protobuf message. - using MessagePtr = ::google::protobuf::Message *; - // Return a pointer to a NovAtel parser. The caller should take ownership. - static Parser *CreateNovatel(const config::Config &config); - - // Return a pointer to rtcm v3 parser. The caller should take ownership. - static Parser *CreateRtcmV3(bool is_base_station = false); - - virtual ~Parser() {} - - // Updates the parser with new data. The caller must keep the data valid until - // GetMessage() - // returns NONE. - void Update(const uint8_t *data, size_t length) { - data_ = data; - data_end_ = data + length; - } - - void Update(const std::string &data) { - Update(reinterpret_cast(data.data()), data.size()); - } + // States for the parsing finite state machine. + enum class ParseState { + SEEK_HEADER, // Looking for the start of a message/packet header. + PROCESS_PAYLOAD // Header found, processing the message payload. + }; + // Types of messages that can be parsed. enum class MessageType { NONE, GNSS, @@ -82,6 +58,7 @@ class Parser { WHEEL, EPHEMERIDES, OBSERVATION, + BDGGA, GPGGA, BDSEPHEMERIDES, RAWIMU, @@ -91,22 +68,223 @@ class Parser { HEADING, }; - // Gets a parsed protobuf message. The caller must consume the message before - // calling another - // GetMessage() or Update(); - virtual MessageType GetMessage(MessagePtr *message_ptr) = 0; + // A general pointer to a protobuf message. Used for parsed results. + using ProtoMessagePtr = std::unique_ptr<::google::protobuf::Message>; + + // Represents a successfully parsed message. + // It's a pair of the message type and a variant holding either a protobuf + // message or raw bytes (for formats not fully converted to protobuf + // internally). + using ParsedMessage = + std::pair>>; + + virtual ~Parser() = default; + + // Static factory method to create a Parser instance based on configuration. + // This method is implemented in the .cpp file. + static std::unique_ptr Create(const config::Config &config); + + bool AppendData(const uint8_t *data, size_t length) { + return buffer_.Append(data, length); + } + + bool AppendData(const std::string &data) { return buffer_.Append(data); } + + // Parses all complete messages currently available in the buffer. + // Calls TryParseMessage repeatedly until no more messages can be parsed + // or no progress is made on the buffer. + // Returns a vector of all successfully parsed messages. + virtual std::vector ParseAllMessages(); protected: - Parser() {} + // Protected constructor enforces creation through the static Create method or + // derived classes. + explicit Parser(size_t buffer_max_size = 0) + : buffer_(buffer_max_size), state_(ParseState::SEEK_HEADER) {} + + // Tries to parse a single message or make progress towards parsing a message + // from the internal buffer based on the current state. + // This method manages the state transitions and calls the virtual methods. + // Returns: + // - An optional vector of ParsedMessage if one or more messages were + // successfully + // parsed in this step (state transitions to SEEK_HEADER). + // - std::nullopt if no complete message was parsed in this call (either + // still seeking header, processing payload, or buffer is incomplete). + std::optional> TryParseMessage(); + + // Pure virtual method to be implemented by derived classes. + // This method is called when the parser is in the SEEK_HEADER state. + // It should search the buffer for a valid header pattern and consume + // any leading garbage data. + // Implementation must interact with 'buffer_'. + // Returns: + // - true if a valid header is found and the buffer is positioned at the start + // of the payload (or the header itself if needed for payload processing). + // - false if no valid header is found in the current buffer content + // (might need more data, or data is invalid). + // If false is returned, the implementation *must* ensure that the buffer + // state is updated appropriately to prevent infinite loops (e.g., consume + // some data if possible, or rely on ParseAllMessages' progress check). + virtual bool ProcessHeader() = 0; + + // Pure virtual method to be implemented by derived classes. + // This method is called when the parser is in the PROCESS_PAYLOAD state. + // It should attempt to extract a complete message payload from the buffer, + // parse it, and generate the corresponding ParsedMessage(s). + // Implementation must interact with 'buffer_'. + // Returns: + // - An optional vector of ParsedMessage if one or more messages were + // successfully + // parsed from the current payload chunk. The implementation must consume + // the corresponding data from 'buffer_'. + // - std::nullopt if the payload is incomplete (need more data) or invalid. + // If std::nullopt is returned due to incompleteness, the buffer should not + // have been fully consumed for this payload. If returned due to invalidity, + // the implementation should handle error recovery and potentially + // consume/skip invalid data to prevent infinite loops. + virtual std::optional> ProcessPayload() = 0; - // Point to the beginning and end of data. Do not take ownership. - const uint8_t *data_ = nullptr; - const uint8_t *data_end_ = nullptr; + // Internal data buffer holding raw input bytes. + DataBuffer buffer_; + + // Current state of the parsing state machine. + ParseState state_; private: - DISABLE_COPY_AND_ASSIGN(Parser); + DISALLOW_COPY_AND_ASSIGN(Parser); }; +std::unique_ptr Parser::Create(const config::Config &config) { + std::unordered_map( + const config::Config &)>> + parser_factories = { + {config::Stream::NOVATEL_BINARY, + [](const config::Config &cfg) { + return std::make_unique(cfg); + }}, + {config::Stream::HUACE_TEXT, + [](const config::Config &cfg) { + return std::make_unique(cfg); + }}, + // Add more parser types here by mapping config::Stream to a lambda + // that creates the corresponding derived parser instance. + }; + + auto factory_it = parser_factories.find(config.data().format()); + if (factory_it != parser_factories.end()) { + AINFO << "Creating parser for format: " + << config::Stream_Name(config.data().format()); + return factory_it->second(config); + } + + AERROR << "Unsupported data format: " + << static_cast(config.data().format()); + return nullptr; +} + +std::optional> Parser::TryParseMessage() { + switch (state_) { + case ParseState::SEEK_HEADER: { + // Try to find and process a header. + if (ProcessHeader()) { + // Header found, transition to processing payload. + state_ = ParseState::PROCESS_PAYLOAD; + // Return nullopt as we haven't processed a full message yet, just the + // header. + return std::nullopt; + } + // Header not found or buffer incomplete, stay in SEEK_HEADER. + return std::nullopt; + } + case ParseState::PROCESS_PAYLOAD: { + // Try to process the payload and get messages. + auto parsed_msg = ProcessPayload(); + if (parsed_msg.has_value()) { + // Payload processed successfully (even if 0 messages were yielded but + // data was consumed). Transition back to seeking the next header. + state_ = ParseState::SEEK_HEADER; + // Return the parsed messages (could be an empty vector if payload was + // just consumption). + return parsed_msg; + } + // Payload incomplete or invalid, stay in PROCESS_PAYLOAD or transition + // based on ProcessPayload's handling. ProcessPayload returning + // std::nullopt indicates insufficient data or unrecoverable error *for + // the current payload*. + return std::nullopt; + } + default: + // Should not happen, but handle unexpected states gracefully. + AINFO << "Unknown parser state: " << static_cast(state_) + << ". Resetting to SEEK_HEADER."; + state_ = ParseState::SEEK_HEADER; + return std::nullopt; + } +} + +std::vector Parser::ParseAllMessages() { + std::vector parsed_messages; + // Loop while we are making progress by either parsing messages + // or consuming data in the buffer. + while (true) { + // Store buffer size before attempting to parse. + size_t initial_buffer_size = buffer_.Size(); + + // Try to parse one unit (header or payload). + auto result = TryParseMessage(); + + if (result.has_value()) { + // Successfully parsed one or more messages from a payload. + std::move(result.value().begin(), result.value().end(), + std::back_inserter(parsed_messages)); + // Continue the loop to try parsing the next message immediately + // from the remaining data. The state has been reset to SEEK_HEADER + // by TryParseMessage. + + } else { + // TryParseMessage returned std::nullopt. + // This means either: + // 1. We are in SEEK_HEADER and couldn't find a header (buffer incomplete + // or no header). + // 2. We are in PROCESS_PAYLOAD and couldn't complete the payload (buffer + // incomplete or invalid). + + // Check if any data was consumed by the call to TryParseMessage (via + // ProcessHeader/ProcessPayload). + if (buffer_.Size() < initial_buffer_size) { + // Data was consumed. This could be: + // - Skipping leading garbage in SEEK_HEADER. + // - Partially processing a header or payload. + // - Recovering from an error in ProcessPayload by skipping invalid + // bytes. Since progress (data consumption) was made, continue the loop + // to try again with the potentially smaller buffer. + AINFO_IF(buffer_.Size() < initial_buffer_size) + << "Parser consumed data without yielding a message. Buffer size " + "decreased from " + << initial_buffer_size << " to " << buffer_.Size(); + continue; // Explicitly continue the loop. + } else { + // No data was consumed, and no message was parsed. + // This indicates that the parser is stuck - it needs more data to + // proceed or the remaining data is insufficient/invalid for *any* + // further processing *right now*. Break the loop as no progress can be + // made with the current buffer content. + AINFO_IF(!buffer_.IsEmpty()) + << "Parser made no progress. Buffer size remains " << buffer_.Size() + << ". Stopping parsing until more data arrives."; + break; + } + } + + // The loop continues implicitly if result.has_value() was true. + // It continues explicitly with 'continue' if data was consumed but no + // message yielded. It breaks if no progress is made. + } + return parsed_messages; +} + } // namespace gnss } // namespace drivers } // namespace apollo diff --git a/modules/drivers/gnss/parser/rtcm3/BUILD b/modules/drivers/gnss/parser/rtcm3/BUILD new file mode 100644 index 00000000000..0d5bdc8fef9 --- /dev/null +++ b/modules/drivers/gnss/parser/rtcm3/BUILD @@ -0,0 +1,16 @@ +load("@rules_cc//cc:defs.bzl", "cc_library") +load("//tools:cpplint.bzl", "cpplint") + +cc_library( + name = "rtcm3_parser", + srcs = [ + "rtcm3_parser.cc", + ], + hdrs = [ + "rtcm3_parser.h", + ], + deps = [ + ], +) + +cpplint() diff --git a/modules/drivers/gnss/parser/rtcm3_parser.cc b/modules/drivers/gnss/parser/rtcm3/rtcm3_parser.cc similarity index 61% rename from modules/drivers/gnss/parser/rtcm3_parser.cc rename to modules/drivers/gnss/parser/rtcm3/rtcm3_parser.cc index 46222d1008d..b69d3547f63 100644 --- a/modules/drivers/gnss/parser/rtcm3_parser.cc +++ b/modules/drivers/gnss/parser/rtcm3/rtcm3_parser.cc @@ -14,38 +14,28 @@ * limitations under the License. *****************************************************************************/ -#include "modules/drivers/gnss/parser/rtcm3_parser.h" +#include "gnss/parser/rtcm3/rtcm3_parser.h" #include +#include "modules/drivers/gnss/util/util.h" + namespace apollo { namespace drivers { namespace gnss { -// Anonymous namespace that contains helper constants and functions. -namespace { - -template -constexpr bool is_zero(T value) { - return value == static_cast(0); -} - -} // namespace - -Parser *Parser::CreateRtcmV3(bool is_base_station) { - return new Rtcm3Parser(is_base_station); -} +Rtcm3Parser::Rtcm3Parser(const config::Config &config) + : Parser(config.data().buffer_size()), + Rtcm3Parser(config.is_base_station()) {} -Rtcm3Parser::Rtcm3Parser(bool is_base_station) { +Rtcm3Parser::Rtcm3Parser(bool is_base_station) + : Parser(4096), is_base_station_(is_base_station), init_flag_(false) { if (1 != init_rtcm(&rtcm_)) { init_flag_ = true; - } else { - init_flag_ = false; } ephemeris_.Clear(); observation_.Clear(); - is_base_station_ = is_base_station; } bool Rtcm3Parser::SetStationPosition() { @@ -152,43 +142,114 @@ void Rtcm3Parser::SetObservationTime() { observation_.set_gnss_second_s(second); } -Parser::MessageType Rtcm3Parser::GetMessage(MessagePtr *message_ptr) { - if (data_ == nullptr) { - return MessageType::NONE; - } +std::vector Rtcm3Parser::ParseAllMessages() { + std::vector parsed_messages; - while (data_ < data_end_) { - const int status = input_rtcm3(&rtcm_, *data_++); // parse data use rtklib + while (auto byte_opt = buffer_.Poll()) { + uint8_t byte = *byte_opt; + const int status = input_rtcm3(&rtcm_, byte); switch (status) { - case 1: // observation data - if (ProcessObservation()) { - *message_ptr = &observation_; - return MessageType::OBSERVATION; + case 0: // No message ready, need more data. Continue loop to feed next + // byte. + // AINFO_EVERY(500) << "input_rtcm3 status 0: Need more data."; + break; // Continue while loop + + case 1: // Observation data ready (e.g., RTCM 1074-1078, 1084-1088, etc.) + AINFO << "input_rtcm3 status 1: Observation data ready. Msg type: " + << rtcm_.msgtype; + // Process the ready observation data from rtcm_ structure + if (ProcessObservation(&observation_)) { // Use adapted helper + // Create ParsedMessage by copying from the internal member + auto msg_ptr = + std::make_unique(); + msg_ptr->CopyFrom(observation_); + parsed_messages.emplace_back(MessageType::OBSERVATION, + std::move(msg_ptr)); } break; - case 2: // ephemeris - if (ProcessEphemerides()) { - *message_ptr = &ephemeris_; - return MessageType::EPHEMERIDES; + case 2: // Ephemeris data ready (e.g., RTCM 1019, 1020, 1042, 1045, 1046, + // etc.) + AINFO << "input_rtcm3 status 2: Ephemeris data ready. Msg type: " + << rtcm_.msgtype; + if (ProcessEphemerides(&ephemeris_)) { + auto msg_ptr = + std::make_unique(); + msg_ptr->CopyFrom(ephemeris_); + parsed_messages.emplace_back(MessageType::EPHEMERIDES, + std::move(msg_ptr)); + // ephemeris_.Clear(); // Optional } break; - case 5: + case 3: // Station Auxiliary Data (e.g. antenna type, etc.) + AINFO + << "input_rtcm3 status 3: Station Auxiliary data ready. Msg type: " + << rtcm_.msgtype; + // If needed, add a ProcessStationAuxiliary() helper and create a + // message here. + break; + + case 4: // Untyped product specific messages + AINFO << "input_rtcm3 status 4: Untyped product specific message. Msg " + "type: " + << rtcm_.msgtype; + // If needed, add a handler for these messages. + break; + + case 5: // Station Position or Grid Info (e.g., RTCM 1005, 1006, 1007, + // 1008) + AINFO << "input_rtcm3 status 5: Station info ready. Msg type: " + << rtcm_.msgtype; ProcessStationParameters(); break; - case 10: // ssr messages - default: + case 10: // SSR messages (State Space Representation) + AINFO_EVERY(100) + << "input_rtcm3 status 10: SSR message ready. Msg type: " + << rtcm_.msgtype; + // SSR messages are complex. If needed, add a HandleSSR() helper here. + break; + + case -1: // Input data error + AERROR_EVERY(100) + << "input_rtcm3 status -1: Input data error processing byte " + << std::hex << static_cast(byte) << std::dec + << ". Buffer size: " << buffer_.Size(); + break; + + case -2: // RTCM message length error + AERROR_EVERY(100) << "input_rtcm3 status -2: RTCM message length error " + "processing byte " + << std::hex << static_cast(byte) << std::dec + << ". Buffer size: " << buffer_.Size(); + break; + + case -3: // RTCM message CRC error + AERROR_EVERY(100) + << "input_rtcm3 status -3: RTCM message CRC error processing byte " + << std::hex << static_cast(byte) << std::dec + << ". Msg type: " << rtcm_.msgtype + << ". Buffer size: " << buffer_.Size(); + // input_rtcm3 likely discards the message. Continue loop. + break; + + default: // Other possible status codes? (Consult RTKLIB docs if needed) + AWARN_EVERY(100) << "input_rtcm3 returned unknown status: " << status + << " processing byte " << std::hex + << static_cast(byte) << std::dec + << ". Msg type: " << rtcm_.msgtype + << ". Buffer size: " << buffer_.Size(); break; } } - return MessageType::NONE; + return parsed_messages; } -bool Rtcm3Parser::ProcessObservation() { +bool Rtcm3Parser::ProcessObservation( + apollo::drivers::gnss::GnssObservation *observation_msg) { if (rtcm_.obs.n == 0) { AWARN << "Obs is zero."; } @@ -261,7 +322,8 @@ bool Rtcm3Parser::ProcessObservation() { return true; } -bool Rtcm3Parser::ProcessEphemerides() { +bool Rtcm3Parser::ProcessEphemerides( + apollo::drivers::gnss::GnssEphemeris *ephemeris_msg) { apollo::drivers::gnss::GnssType gnss_type; if (!gnss_sys(rtcm_.message_type, &gnss_type)) { diff --git a/modules/drivers/gnss/parser/rtcm3_parser.h b/modules/drivers/gnss/parser/rtcm3/rtcm3_parser.h similarity index 72% rename from modules/drivers/gnss/parser/rtcm3_parser.h rename to modules/drivers/gnss/parser/rtcm3/rtcm3_parser.h index 7311942f949..dab488b367c 100644 --- a/modules/drivers/gnss/parser/rtcm3_parser.h +++ b/modules/drivers/gnss/parser/rtcm3/rtcm3_parser.h @@ -23,12 +23,10 @@ #include #include -#include "modules/common_msgs/sensor_msgs/gnss_raw_observation.pb.h" +#include "gnss/parser/parser.h" +#include "gnss/parser/rtcm_decode.h" -#include "cyber/cyber.h" - -#include "modules/drivers/gnss/parser/parser.h" -#include "modules/drivers/gnss/parser/rtcm_decode.h" +#include "gnss/proto/gnss_raw_observation.pb.h" namespace apollo { namespace drivers { @@ -37,7 +35,14 @@ namespace gnss { class Rtcm3Parser : public Parser { public: explicit Rtcm3Parser(bool is_base_satation); - virtual MessageType GetMessage(MessagePtr *message_ptr); + explicit Rtcm3Parser(const config::Config &config); + + virtual std::vector ParseAllMessages() override; + + bool Rtcm3Parser::ProcessHeader() override { return true; } + std::optional> Rtcm3Parser::ProcessPayload() { + return std::nullopt; + } private: void SetObservationTime(); @@ -46,25 +51,26 @@ class Rtcm3Parser : public Parser { apollo::drivers::gnss::KepplerOrbit *keppler_orbit); void FillGlonassOrbit(const geph_t &eph, apollo::drivers::gnss::GlonassOrbit *keppler_orbit); - bool ProcessObservation(); - bool ProcessEphemerides(); + bool ProcessObservation( + apollo::drivers::gnss::GnssObservation *observation_msg); + bool ProcessEphemerides(apollo::drivers::gnss::GnssEphemeris *ephemeris_msg); bool ProcessStationParameters(); - bool init_flag_; - std::vector buffer_; - - rtcm_t rtcm_; + private: bool is_base_station_ = false; + bool init_flag_; - apollo::drivers::gnss::GnssEphemeris ephemeris_; - apollo::drivers::gnss::EpochObservation observation_; + rtcm_t rtcm_; struct Point3D { double x; double y; double z; }; - std::map station_location_; + std::unordered_map station_location_; + + apollo::drivers::gnss::GnssEphemeris ephemeris_; + apollo::drivers::gnss::EpochObservation observation_; }; } // namespace gnss diff --git a/modules/drivers/gnss/parser/rtcm_decode.h b/modules/drivers/gnss/parser/rtcm_decode.h index a697e2293aa..4446391e4f1 100644 --- a/modules/drivers/gnss/parser/rtcm_decode.h +++ b/modules/drivers/gnss/parser/rtcm_decode.h @@ -17,6 +17,7 @@ #pragma once #include + #include "modules/common_msgs/sensor_msgs/gnss_raw_observation.pb.h" namespace apollo { diff --git a/modules/drivers/gnss/parser/rtcm_parser.cc b/modules/drivers/gnss/parser/rtcm_parser.cc index 8c6c9f4758a..48531957d02 100644 --- a/modules/drivers/gnss/parser/rtcm_parser.cc +++ b/modules/drivers/gnss/parser/rtcm_parser.cc @@ -22,7 +22,6 @@ #include "modules/common/adapters/adapter_gflags.h" #include "modules/drivers/gnss/parser/parser.h" #include "modules/drivers/gnss/parser/rtcm3_parser.h" -#include "modules/common_msgs/sensor_msgs/gnss_raw_observation.pb.h" namespace apollo { namespace drivers { @@ -56,16 +55,15 @@ void RtcmParser::ParseRtcmData(const std::string& msg) { return; } - rtcm_parser_->Update(msg); - Parser::MessageType type; - MessagePtr msg_ptr; + rtcm_parser_->AppendData(msg); + auto messages = rtcm_parser_->ParseAllMessages(); - while (cyber::OK()) { - type = rtcm_parser_->GetMessage(&msg_ptr); - if (type == Parser::MessageType::NONE) { - break; + for (const auto& [msg_type, msg_variant] : messages) { + if (std::holds_alternative(msg_variant)) { + DispatchMessage(msg_type, std::get(msg_variant)); + } else { + AERROR << "Unknown message type variant."; } - DispatchMessage(type, msg_ptr); } } diff --git a/modules/drivers/gnss/parser/rtcm_parser.h b/modules/drivers/gnss/parser/rtcm_parser.h index 1cc18d4032a..5e89e75ef65 100644 --- a/modules/drivers/gnss/parser/rtcm_parser.h +++ b/modules/drivers/gnss/parser/rtcm_parser.h @@ -19,12 +19,11 @@ #include #include -#include "cyber/cyber.h" +#include "modules/common_msgs/sensor_msgs/gnss_raw_observation.pb.h" +#include "cyber/cyber.h" #include "modules/drivers/gnss/parser/parser.h" -#include "modules/common_msgs/sensor_msgs/gnss_raw_observation.pb.h" - namespace apollo { namespace drivers { namespace gnss { diff --git a/modules/drivers/gnss/proto/config.proto b/modules/drivers/gnss/proto/config.proto index 621852fb5b9..15e7bc052e1 100644 --- a/modules/drivers/gnss/proto/config.proto +++ b/modules/drivers/gnss/proto/config.proto @@ -12,8 +12,24 @@ message Stream { NOVATEL_TEXT = 10; NOVATEL_BINARY = 11; + HUACE_TEXT = 12; + HUACE_CAN = 13; + + ASENSING_TEXT = 14; + ASENSING_BINARY = 15; + UBLOX_TEXT = 20; UBLOX_BINARY = 21; + + BROADGNSS_TEXT = 30; + BROADGNSS_BINARY = 31; + + ENBROAD_TEXT = 40; + ENBROAD_BINARY = 41; + + FORSENSE_TEXT = 50; + + CXZL_TEXT = 60; } optional Format format = 1; @@ -118,4 +134,6 @@ message Config { // If given, the driver will send velocity info to novatel with command stream optional string wheel_parameters = 13; optional string gpsbin_folder = 14; + // + optional bool is_base_station = 15; } diff --git a/modules/drivers/gnss/stream/BUILD b/modules/drivers/gnss/stream/BUILD index fba6a28cfc0..4d206f3b451 100644 --- a/modules/drivers/gnss/stream/BUILD +++ b/modules/drivers/gnss/stream/BUILD @@ -3,39 +3,12 @@ load("//tools:cpplint.bzl", "cpplint") package(default_visibility = ["//visibility:public"]) -cc_library( - name = "gnss_stream", - deps = [ - ":ntrip_stream", - ":raw_stream", - ":serial_stream", - ":tcp_stream", - ":udp_stream", - ], -) - -cc_library( - name = "ntrip_stream", - srcs = ["ntrip_stream.cc"], - hdrs = ["tcp_stream.h"], - deps = [ - ":stream", - "//cyber", - "//modules/common/adapters:adapter_gflags", - "//modules/common/math", - "//modules/common_msgs/basic_msgs:pnc_point_cc_proto", - "//modules/common/util", - ], -) - cc_library( name = "raw_stream", srcs = ["raw_stream.cc"], hdrs = ["raw_stream.h"], deps = [ - ":ntrip_stream", - ":serial_stream", - ":stream", + "//modules/drivers/hal/stream/stream_factory", "//cyber", "//modules/common_msgs/chassis_msgs:chassis_cc_proto", "//modules/common/adapters:adapter_gflags", @@ -52,43 +25,4 @@ cc_library( ], ) -cc_library( - name = "serial_stream", - srcs = ["serial_stream.cc"], - deps = [ - ":stream", - "//cyber", - "//modules/drivers/gnss/util:gnss_util", - ], -) - -cc_library( - name = "tcp_stream", - srcs = ["tcp_stream.cc"], - hdrs = ["tcp_stream.h"], - deps = [ - ":stream", - "//cyber", - ], -) - -cc_library( - name = "udp_stream", - srcs = ["udp_stream.cc"], - deps = [ - ":stream", - "//cyber", - "//modules/drivers/gnss/util:gnss_util", - ], -) - -cc_library( - name = "stream", - hdrs = ["stream.h"], - deps = [ - "//cyber", - "//modules/drivers/gnss/util:gnss_util", - ], -) - cpplint() diff --git a/modules/drivers/gnss/stream/ntrip_stream.cc b/modules/drivers/gnss/stream/ntrip_stream.cc deleted file mode 100644 index d49aed16e12..00000000000 --- a/modules/drivers/gnss/stream/ntrip_stream.cc +++ /dev/null @@ -1,247 +0,0 @@ -/****************************************************************************** - * Copyright 2017 The Apollo Authors. All Rights Reserved. - * - * 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 "cyber/cyber.h" - -#include "modules/common/util/string_util.h" -#include "modules/common/util/util.h" -#include "modules/drivers/gnss/stream/stream.h" -#include "modules/drivers/gnss/stream/tcp_stream.h" - -namespace { - -template -constexpr bool is_zero(T value) { - return value == static_cast(0); -} -} // namespace - -namespace apollo { -namespace drivers { -namespace gnss { - -class NtripStream : public Stream { - public: - NtripStream(const std::string& address, uint16_t port, - const std::string& mountpoint, const std::string& user, - const std::string& passwd, uint32_t timeout_s); - ~NtripStream(); - - virtual size_t read(uint8_t* buffer, size_t max_length); - virtual size_t write(const uint8_t* data, size_t length); - virtual bool Connect(); - virtual bool Disconnect(); - - private: - void Reconnect(); - bool is_login_ = false; - const std::string mountpoint_; - const std::string write_data_prefix_; - const std::string login_data_; - double timeout_s_ = 60.0; - double data_active_s_ = 0.0; - std::unique_ptr tcp_stream_; - std::mutex internal_mutex_; -}; - -NtripStream::NtripStream(const std::string& address, uint16_t port, - const std::string& mountpoint, const std::string& user, - const std::string& passwd, uint32_t timeout_s) - : mountpoint_(mountpoint), - write_data_prefix_("GET /" + mountpoint + - " HTTP/1.0\r\n" - "User-Agent: NTRIP gnss_driver/0.0\r\n" - "accept: */* \r\n\r\n"), - - login_data_("GET /" + mountpoint + - " HTTP/1.0\r\n" - "User-Agent: NTRIP gnss_driver/0.0\r\n" - "accept: */* \r\n" - "Authorization: Basic " + - common::util::EncodeBase64(user + ":" + passwd) + "\r\n\r\n"), - timeout_s_(timeout_s), - tcp_stream_(new TcpStream(address.c_str(), port, 0, false)) {} - -NtripStream::~NtripStream() { this->Disconnect(); } - -bool NtripStream::Connect() { - if (is_login_) { - return true; - } - if (!tcp_stream_) { - AERROR << "New tcp stream failed."; - return true; - } - - if (!tcp_stream_->Connect()) { - status_ = Stream::Status::DISCONNECTED; - AERROR << "Tcp connect failed."; - return false; - } - - uint8_t buffer[2048]; - size_t size = 0; - size_t try_times = 0; - - size = tcp_stream_->write( - reinterpret_cast(login_data_.data()), login_data_.size()); - if (size != login_data_.size()) { - tcp_stream_->Disconnect(); - status_ = Stream::Status::ERROR; - AERROR << "Send ntrip request failed."; - return false; - } - - bzero(buffer, sizeof(buffer)); - AINFO << "Read ntrip response."; - size = tcp_stream_->read(buffer, sizeof(buffer) - 1); - while ((size == 0) && (try_times < 3)) { - sleep(1); - size = tcp_stream_->read(buffer, sizeof(buffer) - 1); - ++try_times; - } - - if (!size) { - tcp_stream_->Disconnect(); - status_ = Stream::Status::DISCONNECTED; - AERROR << "No response from ntripcaster."; - return false; - } - - if (std::strstr(reinterpret_cast(buffer), "ICY 200 OK\r\n")) { - status_ = Stream::Status::CONNECTED; - is_login_ = true; - AINFO << "Ntrip login successfully."; - return true; - } - - if (std::strstr(reinterpret_cast(buffer), "SOURCETABLE 200 OK\r\n")) { - AERROR << "Mountpoint " << mountpoint_ << " not exist."; - } - - if (std::strstr(reinterpret_cast(buffer), "HTTP/")) { - AERROR << "Authentication failed."; - } - - AINFO << "No expect data."; - AINFO << "Recv data length: " << size; - // AINFO << "Data from server: " << reinterpret_cast(buffer); - - tcp_stream_->Disconnect(); - status_ = Stream::Status::ERROR; - return false; -} - -bool NtripStream::Disconnect() { - if (is_login_) { - bool ret = tcp_stream_->Disconnect(); - if (!ret) { - return false; - } - status_ = Stream::Status::DISCONNECTED; - is_login_ = false; - } - - return true; -} - -void NtripStream::Reconnect() { - AINFO << "Reconnect ntrip caster."; - std::unique_lock lock(internal_mutex_); - Disconnect(); - Connect(); - if (status_ != Stream::Status::CONNECTED) { - AINFO << "Reconnect ntrip caster failed."; - return; - } - - data_active_s_ = cyber::Time::Now().ToSecond(); - AINFO << "Reconnect ntrip caster success."; -} - -size_t NtripStream::read(uint8_t* buffer, size_t max_length) { - if (!tcp_stream_) { - return 0; - } - - size_t ret = 0; - - if (tcp_stream_->get_status() != Stream::Status::CONNECTED) { - Reconnect(); - if (status_ != Stream::Status::CONNECTED) { - return 0; - } - } - - if (is_zero(data_active_s_)) { - data_active_s_ = cyber::Time::Now().ToSecond(); - } - - ret = tcp_stream_->read(buffer, max_length); - if (ret) { - data_active_s_ = cyber::Time::Now().ToSecond(); - } - - // timeout detect - if ((cyber::Time::Now().ToSecond() - data_active_s_) > timeout_s_) { - AINFO << "Ntrip timeout."; - Reconnect(); - } - - return ret; -} - -size_t NtripStream::write(const uint8_t* buffer, size_t length) { - if (!tcp_stream_) { - return 0; - } - std::unique_lock lock(internal_mutex_, std::defer_lock); - if (!lock.try_lock()) { - AINFO << "Try lock failed."; - return 0; - } - - if (tcp_stream_->get_status() != Stream::Status::CONNECTED) { - return 0; - } - - std::string data(reinterpret_cast(buffer), length); - data = write_data_prefix_ + data; - size_t ret = tcp_stream_->write(reinterpret_cast(data.data()), - data.size()); - if (ret != data.size()) { - AERROR << "Send ntrip data size " << data.size() << ", return " << ret; - status_ = Stream::Status::ERROR; - return 0; - } - - return length; -} - -Stream* Stream::create_ntrip(const std::string& address, uint16_t port, - const std::string& mountpoint, - const std::string& user, const std::string& passwd, - uint32_t timeout_s) { - return new NtripStream(address, port, mountpoint, user, passwd, timeout_s); -} - -} // namespace gnss -} // namespace drivers -} // namespace apollo diff --git a/modules/drivers/gnss/stream/raw_stream.cc b/modules/drivers/gnss/stream/raw_stream.cc index 6305306d2c2..5fdc5be5b85 100644 --- a/modules/drivers/gnss/stream/raw_stream.cc +++ b/modules/drivers/gnss/stream/raw_stream.cc @@ -14,6 +14,8 @@ * limitations under the License. *****************************************************************************/ +#include "modules/drivers/gnss/stream/raw_stream.h" + #include #include #include @@ -22,12 +24,12 @@ #include "absl/strings/str_cat.h" +#include "modules/drivers/gnss/proto/config.pb.h" + #include "cyber/cyber.h" #include "modules/common/adapters/adapter_gflags.h" #include "modules/common/util/message_util.h" -#include "modules/drivers/gnss/proto/config.pb.h" -#include "modules/drivers/gnss/stream/raw_stream.h" -#include "modules/drivers/gnss/stream/stream.h" +#include "modules/drivers/hal/stream/stream_factory.h" namespace apollo { namespace drivers { @@ -80,8 +82,8 @@ Stream *create_stream(const config::Stream &sd) { << sd.serial().baud_rate(); return nullptr; } - return Stream::create_serial(sd.serial().device().c_str(), - sd.serial().baud_rate()); + return StreamFactory::CreateSerial(sd.serial().device(), + sd.serial().baud_rate()); case config::Stream::kTcp: if (!sd.tcp().has_address()) { @@ -92,8 +94,8 @@ Stream *create_stream(const config::Stream &sd) { AERROR << "tcp def has no port field."; return nullptr; } - return Stream::create_tcp(sd.tcp().address().c_str(), - static_cast(sd.tcp().port())); + return StreamFactory::CreateTcp(sd.tcp().address(), + static_cast(sd.tcp().port())); case config::Stream::kUdp: if (!sd.udp().has_address()) { @@ -104,8 +106,8 @@ Stream *create_stream(const config::Stream &sd) { AERROR << "tcp def has no port field."; return nullptr; } - return Stream::create_udp(sd.udp().address().c_str(), - static_cast(sd.udp().port())); + return StreamFactory::CreateUdp(sd.udp().address(), + static_cast(sd.udp().port())); case config::Stream::kNtrip: if (!sd.ntrip().has_address()) { @@ -128,7 +130,7 @@ Stream *create_stream(const config::Stream &sd) { AERROR << "ntrip def has no passwd field."; return nullptr; } - return Stream::create_ntrip( + return StreamFactory::CreateNtrip( sd.ntrip().address(), static_cast(sd.ntrip().port()), sd.ntrip().mount_point(), sd.ntrip().user(), sd.ntrip().password(), sd.ntrip().timeout_s()); @@ -490,7 +492,7 @@ void RawStream::DataSpin() { raw_writer_->Write(msg_pub); data_parser_ptr_->ParseRawData(msg_pub->data()); if (push_location_) { - PushGpgga(length); + PushGGA(); } } StreamStatusCheck(); @@ -529,23 +531,17 @@ void RawStream::PublishRtkData(const size_t length) { rtcm_parser_ptr_->ParseRtcmData(rtk_msg->data()); } -void RawStream::PushGpgga(const size_t length) { - if (!in_rtk_stream_) { +void RawStream::PushGGA() { + ACHECK_NOTNULL(in_rtk_stream_); + + auto message = data_parser_ptr_->TryGetMessage(Parser::MessageType::GPGGA); + if (!message.value()) { + AERROR << "GPGGA message is null."; return; } - char *gpgga = strstr(reinterpret_cast(buffer_), "$GPGGA"); - if (gpgga) { - char *p = strchr(gpgga, '*'); - if (p) { - p += 5; - if (size_t(p - reinterpret_cast(buffer_)) <= length) { - AINFO_EVERY(5) << "Push gpgga."; - in_rtk_stream_->write(reinterpret_cast(gpgga), - reinterpret_cast(p) - buffer_); - } - } - } + AINFO_EVERY(10) << "Push gpgga."; + in_rtk_stream_->write(message.data(), message.size()); } void RawStream::GpsbinCallback(const std::shared_ptr &raw_data) { diff --git a/modules/drivers/gnss/stream/raw_stream.h b/modules/drivers/gnss/stream/raw_stream.h index f9fa0f14dda..90ed0a275bc 100644 --- a/modules/drivers/gnss/stream/raw_stream.h +++ b/modules/drivers/gnss/stream/raw_stream.h @@ -21,15 +21,14 @@ #include #include -#include "cyber/cyber.h" - #include "modules/common_msgs/chassis_msgs/chassis.pb.h" #include "modules/drivers/gnss/proto/config.pb.h" #include "modules/drivers/gnss/proto/gnss_status.pb.h" +#include "cyber/cyber.h" #include "modules/drivers/gnss/parser/data_parser.h" #include "modules/drivers/gnss/parser/rtcm_parser.h" -#include "modules/drivers/gnss/stream/stream.h" +#include "modules/drivers/hal/stream/stream.h" namespace apollo { namespace drivers { @@ -58,7 +57,7 @@ class RawStream { bool Logout(); void StreamStatusCheck(); void PublishRtkData(size_t length); - void PushGpgga(size_t length); + void PushGGA(); void GpsbinSpin(); void GpsbinCallback(const std::shared_ptr& raw_data); void OnWheelVelocityTimer(); @@ -92,7 +91,6 @@ class RawStream { std::unique_ptr rtk_thread_ptr_; std::unique_ptr data_parser_ptr_; std::unique_ptr rtcm_parser_ptr_; - std::unique_ptr gpsbin_thread_ptr_; std::unique_ptr gpsbin_stream_ = nullptr; std::shared_ptr node_ = nullptr; diff --git a/modules/drivers/gnss/stream/serial_stream.cc b/modules/drivers/gnss/stream/serial_stream.cc deleted file mode 100644 index 5c9feba437c..00000000000 --- a/modules/drivers/gnss/stream/serial_stream.cc +++ /dev/null @@ -1,439 +0,0 @@ -/****************************************************************************** - * Copyright 2017 The Apollo Authors. All Rights Reserved. - * - * 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 -#include -#include -#include -#include -#include -#include -#include -#include - -#include "cyber/cyber.h" - -#include "modules/drivers/gnss/stream/stream.h" - -namespace apollo { -namespace drivers { -namespace gnss { - -speed_t get_serial_baudrate(uint32_t rate) { - switch (rate) { - case 9600: - return B9600; - case 19200: - return B19200; - case 38400: - return B38400; - case 57600: - return B57600; - case 115200: - return B115200; - case 230400: - return B230400; - case 460800: - return B460800; - case 921600: - return B921600; - default: - return 0; - } -} - -class SerialStream : public Stream { - public: - SerialStream(const char* device_name, speed_t baud_rate, - uint32_t timeout_usec); - ~SerialStream(); - - virtual bool Connect(); - virtual bool Disconnect(); - virtual size_t read(uint8_t* buffer, size_t max_length); - virtual size_t write(const uint8_t* data, size_t length); - - private: - SerialStream() {} - void open(); - void close(); - bool configure_port(int fd); - bool wait_readable(uint32_t timeout_us); - bool wait_writable(uint32_t timeout_us); - void check_remove(); - - std::string device_name_; - speed_t baud_rate_; - uint32_t bytesize_; - uint32_t parity_; - uint32_t stopbits_; - uint32_t flowcontrol_; - uint32_t byte_time_us_; - - uint32_t timeout_usec_; - int fd_; - int errno_; - bool is_open_; -}; - -SerialStream::SerialStream(const char* device_name, speed_t baud_rate, - uint32_t timeout_usec) - : device_name_(device_name), - baud_rate_(baud_rate), - bytesize_(8), - parity_(0), - stopbits_(1), - flowcontrol_(0), - timeout_usec_(timeout_usec), - fd_(-1), - errno_(0), - is_open_(false) { - if (device_name_.empty()) { - status_ = Stream::Status::ERROR; - } -} - -SerialStream::~SerialStream() { this->close(); } - -void SerialStream::open(void) { - int fd = 0; - fd = ::open(device_name_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); - if (fd == -1) { - switch (errno) { - case EINTR: - // Recurse because this is a recoverable error. - return open(); - - case ENFILE: - case EMFILE: - default: - AERROR << "Open device " << device_name_ - << " failed, error: " << strerror(errno); - return; - } - } - - if (!configure_port(fd)) { - ::close(fd); - return; - } - - fd_ = fd; - is_open_ = true; -} - -bool SerialStream::configure_port(int fd) { - if (fd < 0) { - return false; - } - - struct termios options; // The options for the file descriptor - if (tcgetattr(fd, &options) == -1) { - AERROR << "tcgetattr failed."; - return false; - } - - // set up raw mode / no echo / binary - options.c_cflag |= (tcflag_t)(CLOCAL | CREAD); - options.c_lflag &= (tcflag_t) ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | - ISIG | IEXTEN); // |ECHOPRT - - options.c_oflag &= (tcflag_t) ~(OPOST); - options.c_iflag &= (tcflag_t) ~(INLCR | IGNCR | ICRNL | IGNBRK); - -#ifdef IUCLC - options.c_iflag &= (tcflag_t)~IUCLC; -#endif - -#ifdef PARMRK - options.c_iflag &= (tcflag_t)~PARMRK; -#endif - -#ifdef BSD_SOURCE_ // depend glibc - ::cfsetspeed(&options, baud_rate_); -#else - ::cfsetispeed(&options, baud_rate_); - ::cfsetospeed(&options, baud_rate_); -#endif - - // setup char len - options.c_cflag &= (tcflag_t)~CSIZE; - - // eight bits - options.c_cflag |= CS8; - - // setup stopbits:stopbits_one - options.c_cflag &= (tcflag_t) ~(CSTOPB); - - // setup parity: parity_none - options.c_iflag &= (tcflag_t) ~(INPCK | ISTRIP); - options.c_cflag &= (tcflag_t) ~(PARENB | PARODD); - -// setup flow control : flowcontrol_none -// xonxoff -#ifdef IXANY - options.c_iflag &= (tcflag_t) ~(IXON | IXOFF | IXANY); -#else - options.c_iflag &= (tcflag_t) ~(IXON | IXOFF); -#endif - -// rtscts -#ifdef CRTSCTS - options.c_cflag &= static_cast(~(CRTSCTS)); -#elif defined CNEW_RTSCTS - options.c_cflag &= static_cast(~(CNEW_RTSCTS)); -#else -#error "OS Support seems wrong." -#endif - - // http://www.unixwiz.net/techtips/termios-vmin-vtime.html - // this basically sets the read call up to be a polling read, - // but we are using select to ensure there is data available - // to read before each call, so we should never needlessly poll - options.c_cc[VMIN] = 0; - options.c_cc[VTIME] = 0; - - // activate settings - ::tcsetattr(fd, TCSANOW, &options); - - // Update byte_time_ based on the new settings. - uint32_t bit_time_us = static_cast(1e6 / 115200); - byte_time_us_ = bit_time_us * (1 + bytesize_ + parity_ + stopbits_); - return true; -} - -bool SerialStream::Connect() { - if (!is_open_) { - this->open(); - if (!is_open_) { - status_ = Stream::Status::ERROR; - errno_ = errno; - return false; - } - } - - if (status_ == Stream::Status::CONNECTED) { - return true; - } - - status_ = Stream::Status::CONNECTED; - Login(); - return true; -} - -void SerialStream::close(void) { - if (is_open_) { - ::close(fd_); - fd_ = -1; - is_open_ = false; - status_ = Stream::Status::DISCONNECTED; - } -} - -bool SerialStream::Disconnect() { - if (!is_open_) { - // not open - return false; - } - - this->close(); - return true; -} - -void SerialStream::check_remove() { - char data = 0; - ssize_t nsent = ::write(fd_, &data, 0); - if (nsent < 0) { - AERROR << "Serial stream detect write failed, error: " << strerror(errno); - switch (errno) { - case EBADF: - case EIO: - status_ = Stream::Status::DISCONNECTED; - AERROR << "Device " << device_name_ << " removed."; - Disconnect(); - break; - } - } -} - -size_t SerialStream::read(uint8_t* buffer, size_t max_length) { - if (!is_open_) { - if (!Connect()) { - return 0; - } - AINFO << "Connect " << device_name_ << " success."; - } - - ssize_t bytes_read = 0; - ssize_t bytes_current_read = 0; - - wait_readable(10000); // wait 10ms - - while (max_length > 0) { - bytes_current_read = ::read(fd_, buffer, max_length); - if (bytes_current_read < 0) { - switch (errno) { - case EAGAIN: - case EINVAL: - bytes_current_read = 0; - break; - - case EBADF: - case EIO: - AERROR << "Serial stream read data failed, error: " - << strerror(errno); - Disconnect(); - if (Connect()) { - AINFO << "Reconnect " << device_name_ << " success."; - bytes_current_read = 0; - break; // has recoverable - } - - default: - AERROR << "Serial stream read data failed, error: " << strerror(errno) - << ", errno: " << errno; - status_ = Stream::Status::ERROR; - errno_ = errno; - return bytes_read; - } - } - - if (bytes_current_read == 0) { - if (!bytes_read) { - check_remove(); - return 0; - } - return bytes_read; - } - max_length -= bytes_current_read; - buffer += bytes_current_read; - bytes_read += bytes_current_read; - } - - return bytes_read; -} - -size_t SerialStream::write(const uint8_t* data, size_t length) { - if (!is_open_) { - if (!Connect()) { - return 0; - } - AINFO << "Connect " << device_name_ << " success."; - } - - size_t total_nsent = 0; - size_t delay_times = 0; - - while ((length > 0) && (delay_times < 5)) { - ssize_t nsent = ::write(fd_, data, length); - if (nsent < 0) { - AERROR << "Serial stream write data failed, error: " << strerror(errno); - switch (errno) { - case EAGAIN: - case EINVAL: - nsent = 0; - break; - - case EBADF: - case EIO: - Disconnect(); - if (Connect()) { - AINFO << "Reconnect " << device_name_ << "success."; - nsent = 0; - break; // has recoverable - } - - default: - status_ = Stream::Status::ERROR; - errno_ = errno; - return total_nsent; - } - } - - if (nsent == 0) { - if (!wait_writable(byte_time_us_)) { - break; - } - ++delay_times; - continue; - } - - total_nsent += nsent; - length -= nsent; - data += nsent; - } - - return total_nsent; -} - -bool SerialStream::wait_readable(uint32_t timeout_us) { - // Setup a select call to block for serial data or a timeout - timespec timeout_ts; - fd_set readfds; - FD_ZERO(&readfds); - FD_SET(fd_, &readfds); - - timeout_ts.tv_sec = timeout_us / 1000000; - timeout_ts.tv_nsec = (timeout_us % 1000000) * 1000; - int r = pselect(fd_ + 1, &readfds, NULL, NULL, &timeout_ts, NULL); - if (r <= 0) { - return false; - } - - // This shouldn't happen, if r > 0 our fd has to be in the list! - if (!FD_ISSET(fd_, &readfds)) { - return false; - } - // Data available to read. - return true; -} - -bool SerialStream::wait_writable(uint32_t timeout_us) { - // Setup a select call to block for serial data or a timeout - timespec timeout_ts; - fd_set writefds; - FD_ZERO(&writefds); - FD_SET(fd_, &writefds); - - timeout_ts.tv_sec = timeout_us / 1000000; - timeout_ts.tv_nsec = (timeout_us % 1000000) * 1000; - int r = pselect(fd_ + 1, NULL, &writefds, NULL, &timeout_ts, NULL); - if (r <= 0) { - return false; - } - - // This shouldn't happen, if r > 0 our fd has to be in the list! - if (!FD_ISSET(fd_, &writefds)) { - return false; - } - // Data available to write. - return true; -} - -Stream* Stream::create_serial(const char* device_name, uint32_t baud_rate, - uint32_t timeout_usec) { - speed_t baud = get_serial_baudrate(baud_rate); - return baud == 0 ? nullptr - : new SerialStream(device_name, baud, timeout_usec); -} - -} // namespace gnss -} // namespace drivers -} // namespace apollo diff --git a/modules/drivers/gnss/stream/stream.h b/modules/drivers/gnss/stream/stream.h deleted file mode 100644 index c16d0b2808a..00000000000 --- a/modules/drivers/gnss/stream/stream.h +++ /dev/null @@ -1,110 +0,0 @@ -/****************************************************************************** - * Copyright 2017 The Apollo Authors. All Rights Reserved. - * - * 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. - *****************************************************************************/ - -// This defines an stream interface for communication via USB, Ethernet, etc. - -#pragma once - -#include -#include -#include - -#include "cyber/cyber.h" - -#include "modules/drivers/gnss/util/macros.h" - -namespace apollo { -namespace drivers { -namespace gnss { - -// An abstract class of Stream. -// One should use the create_xxx() functions to create a Stream object. -class Stream { - public: - // Return a pointer to a Stream object. The caller should take ownership. - static Stream *create_tcp(const char *address, uint16_t port, - uint32_t timeout_usec = 1000000); - - static Stream *create_udp(const char *address, uint16_t port, - uint32_t timeout_usec = 1000000); - - // Currently the following baud rates are supported: - // 9600, 19200, 38400, 57600, 115200, 230400, 460800, 921600. - static Stream *create_serial(const char *device_name, uint32_t baud_rate, - uint32_t timeout_usec = 0); - - static Stream *create_ntrip(const std::string &address, uint16_t port, - const std::string &mountpoint, - const std::string &user, - const std::string &passwd, - uint32_t timeout_s = 30); - - virtual ~Stream() {} - - // Stream status. - enum class Status { - DISCONNECTED, - CONNECTED, - ERROR, - }; - - static constexpr size_t NUM_STATUS = - static_cast(Stream::Status::ERROR) + 1; - Status get_status() const { return status_; } - - // Returns whether it was successful to connect. - virtual bool Connect() = 0; - - // Returns whether it was successful to disconnect. - virtual bool Disconnect() = 0; - - void RegisterLoginData(const std::vector login_data) { - login_data_.assign(login_data.begin(), login_data.end()); - } - - void Login() { - for (size_t i = 0; i < login_data_.size(); ++i) { - write(login_data_[i]); - AINFO << "Login: " << login_data_[i]; - // sleep a little to avoid overrun of the slow serial interface. - cyber::Duration(0.5).Sleep(); - } - } - - // Reads up to max_length bytes. Returns actually number of bytes read. - virtual size_t read(uint8_t *buffer, size_t max_length) = 0; - - // Returns how many bytes it was successful to write. - virtual size_t write(const uint8_t *buffer, size_t length) = 0; - - size_t write(const std::string &buffer) { - return write(reinterpret_cast(buffer.data()), - buffer.size()); - } - - protected: - Stream() {} - - Status status_ = Status::DISCONNECTED; - - private: - std::vector login_data_; - DISABLE_COPY_AND_ASSIGN(Stream); -}; - -} // namespace gnss -} // namespace drivers -} // namespace apollo diff --git a/modules/drivers/gnss/stream/tcp_stream.cc b/modules/drivers/gnss/stream/tcp_stream.cc deleted file mode 100644 index fbab1807f04..00000000000 --- a/modules/drivers/gnss/stream/tcp_stream.cc +++ /dev/null @@ -1,351 +0,0 @@ -/****************************************************************************** - * Copyright 2017 The Apollo Authors. All Rights Reserved. - * - * 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 -#include -#include - -#include -#include -#include - -#include "cyber/cyber.h" -#include "modules/drivers/gnss/stream/stream.h" -#include "modules/drivers/gnss/stream/tcp_stream.h" - -namespace apollo { -namespace drivers { -namespace gnss { - -TcpStream::TcpStream(const char* address, uint16_t port, uint32_t timeout_usec, - bool auto_reconnect) - : sockfd_(-1), errno_(0), auto_reconnect_(auto_reconnect) { - peer_addr_ = inet_addr(address); - peer_port_ = htons(port); - timeout_usec_ = timeout_usec; -} - -TcpStream::~TcpStream() { this->close(); } - -void TcpStream::open() { - int fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); - if (fd < 0) { - // error - AERROR << "create socket failed, errno: " << errno << ", " - << strerror(errno); - return; - } - - sockfd_ = fd; -} - -bool TcpStream::InitSocket() { - if (sockfd_ < 0) { - return false; - } - - // block or not block - if (timeout_usec_ != 0) { - int flags = fcntl(sockfd_, F_GETFL, 0); - if (flags == -1) { - ::close(sockfd_); - AERROR << "fcntl get flag failed, error: " << strerror(errno); - return false; - } - - if (fcntl(sockfd_, F_SETFL, flags & ~O_NONBLOCK) == -1) { - ::close(sockfd_); - AERROR << "fcntl set block failed, error: " << strerror(errno); - return false; - } - - timeval block_to = {timeout_usec_ / 1000000, timeout_usec_ % 1000000}; - if (setsockopt(sockfd_, SOL_SOCKET, SO_RCVTIMEO, &block_to, - sizeof(block_to)) < 0) { - ::close(sockfd_); - AERROR << "setsockopt set rcv timeout failed, error: " << strerror(errno); - return false; - } - - if (setsockopt(sockfd_, SOL_SOCKET, SO_SNDTIMEO, &block_to, - sizeof(block_to)) < 0) { - ::close(sockfd_); - AERROR << "setsockopt set snd timeout failed, error: " << strerror(errno); - return false; - } - } else { - int flags = fcntl(sockfd_, F_GETFL, 0); - if (flags == -1) { - ::close(sockfd_); - AERROR << "fcntl get flag failed, error: " << strerror(errno); - return false; - } - - if (fcntl(sockfd_, F_SETFL, flags | O_NONBLOCK) == -1) { - ::close(sockfd_); - AERROR << "fcntl set non block failed, error: " << strerror(errno); - return false; - } - } - - // disable Nagle - int ret = 0; - int enable = 1; - ret = setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, - reinterpret_cast(&enable), sizeof(enable)); - if (ret == -1) { - ::close(sockfd_); - AERROR << "setsockopt disable Nagle failed, errno: " << errno << ", " - << strerror(errno); - return false; - } - - return true; -} - -void TcpStream::close() { - if (sockfd_ > 0) { - ::close(sockfd_); - sockfd_ = -1; - status_ = Stream::Status::DISCONNECTED; - } -} - -bool TcpStream::Reconnect() { - if (auto_reconnect_) { - Disconnect(); - if (Connect()) { - return true; - } - } - return false; -} - -bool TcpStream::Connect() { - if (sockfd_ < 0) { - this->open(); - if (sockfd_ < 0) { - // error - return false; - } - } - - if (status_ == Stream::Status::CONNECTED) { - return true; - } - - fd_set fds; - timeval timeo = {10, 0}; - int ret = 0; - sockaddr_in peer_addr; - - bzero(&peer_addr, sizeof(peer_addr)); - peer_addr.sin_family = AF_INET; - peer_addr.sin_port = peer_port_; - peer_addr.sin_addr.s_addr = peer_addr_; - - int fd_flags = fcntl(sockfd_, F_GETFL); - if (fd_flags < 0 || fcntl(sockfd_, F_SETFL, fd_flags | O_NONBLOCK) < 0) { - AERROR << "Failed to set noblock, error: " << strerror(errno); - return false; - } - - while ((ret = ::connect(sockfd_, reinterpret_cast(&peer_addr), - sizeof(peer_addr))) < 0) { - if (errno == EINTR) { - AINFO << "Tcp connect return EINTR, continue."; - continue; - } else { - if ((errno != EISCONN) && (errno != EINPROGRESS) && (errno != EALREADY)) { - status_ = Stream::Status::ERROR; - errno_ = errno; - AERROR << "Connect failed, error: " << strerror(errno); - return false; - } - - FD_ZERO(&fds); - FD_SET(sockfd_, &fds); - ret = select(sockfd_ + 1, NULL, &fds, NULL, &timeo); - if (ret < 0) { - status_ = Stream::Status::ERROR; - errno_ = errno; - AERROR << "Wait connect failed, error: " << strerror(errno); - return false; - } else if (ret == 0) { - AINFO << "Tcp connect timeout."; - return false; - } else if (FD_ISSET(sockfd_, &fds)) { - int error = 0; - socklen_t len = sizeof(int); - - if (getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &error, &len) < 0) { - status_ = Stream::Status::ERROR; - errno_ = errno; - AERROR << "Getsockopt failed, error: " << strerror(errno); - return false; - } - if (error != 0) { - status_ = Stream::Status::ERROR; - errno_ = errno; - AERROR << "Socket error: " << strerror(errno); - return false; - } - - // connect successfully - break; - } else { - status_ = Stream::Status::ERROR; - errno_ = errno; - AERROR << "Should not be here."; - return false; - } - } - } - - if (!InitSocket()) { - close(); - status_ = Stream::Status::ERROR; - errno_ = errno; - AERROR << "Failed to init socket."; - return false; - } - AINFO << "Tcp connect success."; - status_ = Stream::Status::CONNECTED; - Login(); - return true; -} - -bool TcpStream::Disconnect() { - if (sockfd_ < 0) { - // not open - return false; - } - - this->close(); - return true; -} - -size_t TcpStream::read(uint8_t* buffer, size_t max_length) { - ssize_t ret = 0; - - if (status_ != Stream::Status::CONNECTED) { - Reconnect(); - if (status_ != Stream::Status::CONNECTED) { - return 0; - } - } - - if (!Readable(10000)) { - return 0; - } - - while ((ret = ::recv(sockfd_, buffer, max_length, 0)) < 0) { - if (errno == EINTR) { - continue; - } else { - // error - if (errno != EAGAIN) { - status_ = Stream::Status::ERROR; - errno_ = errno; - AERROR << "Read errno " << errno << ", error " << strerror(errno); - } - } - - return 0; - } - - if (ret == 0) { - status_ = Stream::Status::ERROR; - errno_ = errno; - AERROR << "Remote closed."; - if (Reconnect()) { - AINFO << "Reconnect tcp success."; - } - } - - return ret; -} - -size_t TcpStream::write(const uint8_t* buffer, size_t length) { - size_t total_nsent = 0; - - if (status_ != Stream::Status::CONNECTED) { - Reconnect(); - if (status_ != Stream::Status::CONNECTED) { - return 0; - } - } - - while (length > 0) { - ssize_t nsent = ::send(sockfd_, buffer, length, 0); - if (nsent < 0) { - if (errno == EINTR) { - continue; - } else { - // error - if (errno == EPIPE || errno == ECONNRESET) { - status_ = Stream::Status::DISCONNECTED; - errno_ = errno; - } else if (errno != EAGAIN) { - status_ = Stream::Status::ERROR; - errno_ = errno; - } - return total_nsent; - } - } - - total_nsent += nsent; - length -= nsent; - buffer += nsent; - } - - return total_nsent; -} - -bool TcpStream::Readable(uint32_t timeout_us) { - // Setup a select call to block for serial data or a timeout - timespec timeout_ts; - fd_set readfds; - FD_ZERO(&readfds); - FD_SET(sockfd_, &readfds); - - timeout_ts.tv_sec = timeout_us / 1000000; - timeout_ts.tv_nsec = (timeout_us % 1000000) * 1000; - int r = pselect(sockfd_ + 1, &readfds, NULL, NULL, &timeout_ts, NULL); - if (r < 0) { - status_ = Stream::Status::ERROR; - errno_ = errno; - AERROR << "Failed to wait tcp data: " << errno << ", " << strerror(errno); - return false; - } else if (r == 0 || !FD_ISSET(sockfd_, &readfds)) { - return false; - } - // Data available to read. - return true; -} - -Stream* Stream::create_tcp(const char* address, uint16_t port, - uint32_t timeout_usec) { - return new TcpStream(address, port, timeout_usec); -} - -} // namespace gnss -} // namespace drivers -} // namespace apollo diff --git a/modules/drivers/gnss/stream/udp_stream.cc b/modules/drivers/gnss/stream/udp_stream.cc deleted file mode 100644 index f4354511166..00000000000 --- a/modules/drivers/gnss/stream/udp_stream.cc +++ /dev/null @@ -1,237 +0,0 @@ -/****************************************************************************** - * Copyright 2017 The Apollo Authors. All Rights Reserved. - * - * 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 -#include -#include - -#include "cyber/cyber.h" - -#include "modules/drivers/gnss/stream/stream.h" - -namespace apollo { -namespace drivers { -namespace gnss { - -class UdpStream : public Stream { - typedef uint16_t be16_t; - typedef uint32_t be32_t; - - public: - UdpStream(const char* address, uint16_t port, uint32_t timeout_usec); - ~UdpStream(); - - virtual bool Connect(); - virtual bool Disconnect(); - virtual size_t read(uint8_t* buffer, size_t max_length); - virtual size_t write(const uint8_t* data, size_t length); - - private: - UdpStream() {} - void open(); - void close(); - be16_t peer_port_ = 0; - be32_t peer_addr_ = 0; - uint32_t timeout_usec_ = 0; - int sockfd_ = -1; - int errno_ = 0; -}; - -Stream* Stream::create_udp(const char* address, uint16_t port, - uint32_t timeout_usec) { - return new UdpStream(address, port, timeout_usec); -} - -UdpStream::UdpStream(const char* address, uint16_t port, uint32_t timeout_usec) - : sockfd_(-1), errno_(0) { - peer_addr_ = inet_addr(address); - peer_port_ = htons(port); - timeout_usec_ = timeout_usec; - // call open or call open in connect later -} - -UdpStream::~UdpStream() { this->close(); } - -void UdpStream::open() { - int fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); - if (fd < 0) { - // error - AERROR << "Create socket failed, errno: " << errno << ", " - << strerror(errno); - return; - } - - // block or not block - if (timeout_usec_ != 0) { - int flags = fcntl(fd, F_GETFL, 0); - if (flags == -1) { - ::close(fd); - AERROR << "fcntl get flag failed, errno: " << errno << ", " - << strerror(errno); - return; - } - - if (fcntl(fd, F_SETFL, flags & ~O_NONBLOCK) == -1) { - ::close(fd); - AERROR << "fcntl set block failed, errno: " << errno << ", " - << strerror(errno); - return; - } - - struct timeval block_to = {timeout_usec_ / 1000000, - timeout_usec_ % 1000000}; - if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, - reinterpret_cast(&block_to), sizeof(block_to)) < 0) { - ::close(fd); - AERROR << "setsockopt set rcv timeout failed, errno: " << errno << ", " - << strerror(errno); - return; - } - - if (setsockopt(fd, SOL_SOCKET, SO_SNDTIMEO, - reinterpret_cast(&block_to), sizeof(block_to)) < 0) { - ::close(fd); - AERROR << "setsockopt set snd timeout failed, errno: " << errno << ", " - << strerror(errno); - return; - } - } else { - int flags = fcntl(fd, F_GETFL, 0); - if (flags == -1) { - ::close(fd); - AERROR << "fcntl get flag failed, errno: " << errno << ", " - << strerror(errno); - return; - } - - if (fcntl(fd, F_SETFL, flags | O_NONBLOCK) == -1) { - ::close(fd); - AERROR << "fcntl set non block failed, errno: " << errno << ", " - << strerror(errno); - return; - } - } - - sockfd_ = fd; -} - -void UdpStream::close() { - if (sockfd_ > 0) { - ::close(sockfd_); - sockfd_ = -1; - status_ = Stream::Status::DISCONNECTED; - } -} - -bool UdpStream::Connect() { - if (sockfd_ < 0) { - this->open(); - if (sockfd_ < 0) { - return false; - } - } - - if (status_ == Stream::Status::CONNECTED) { - return true; - } - - // upper layer support ping method ?? - Login(); - status_ = Stream::Status::CONNECTED; - return true; -} - -bool UdpStream::Disconnect() { - if (sockfd_ < 0) { - // not open - return false; - } - - this->close(); - return true; -} - -size_t UdpStream::read(uint8_t* buffer, size_t max_length) { - ssize_t ret = 0; - struct sockaddr_in peer_sockaddr; - socklen_t socklenth = sizeof(peer_sockaddr); - bzero(&peer_sockaddr, sizeof(peer_sockaddr)); - peer_sockaddr.sin_family = AF_INET; - peer_sockaddr.sin_port = peer_port_; - peer_sockaddr.sin_addr.s_addr = peer_addr_; - - while ((ret = ::recvfrom(sockfd_, buffer, max_length, 0, - (struct sockaddr*)&peer_sockaddr, - reinterpret_cast(&socklenth))) < 0) { - if (errno == EINTR) { - continue; - } else { - // error - if (errno != EAGAIN) { - status_ = Stream::Status::ERROR; - errno_ = errno; - } - } - - return 0; - } - - return ret; -} - -size_t UdpStream::write(const uint8_t* data, size_t length) { - size_t total_nsent = 0; - struct sockaddr_in peer_sockaddr; - bzero(&peer_sockaddr, sizeof(peer_sockaddr)); - peer_sockaddr.sin_family = AF_INET; - peer_sockaddr.sin_port = peer_port_; - peer_sockaddr.sin_addr.s_addr = peer_addr_; - - while (length > 0) { - ssize_t nsent = - ::sendto(sockfd_, data, length, 0, (struct sockaddr*)&peer_sockaddr, - (socklen_t)sizeof(peer_sockaddr)); - if (nsent < 0) { // error - if (errno == EINTR) { - continue; - } else { - // error - if (errno == EPIPE || errno == ECONNRESET) { - status_ = Stream::Status::DISCONNECTED; - errno_ = errno; - } else if (errno != EAGAIN) { - status_ = Stream::Status::ERROR; - errno_ = errno; - } - return total_nsent; - } - } - - total_nsent += nsent; - length -= nsent; - data += nsent; - } - - return total_nsent; -} - -} // namespace gnss -} // namespace drivers -} // namespace apollo diff --git a/modules/drivers/gnss/util/BUILD b/modules/drivers/gnss/util/BUILD index e7bb60b44b3..5142cbd611d 100644 --- a/modules/drivers/gnss/util/BUILD +++ b/modules/drivers/gnss/util/BUILD @@ -1,15 +1,61 @@ -load("@rules_cc//cc:defs.bzl", "cc_library") +load("@rules_cc//cc:defs.bzl", "cc_library", "cc_test") load("//tools:cpplint.bzl", "cpplint") package(default_visibility = ["//visibility:public"]) cc_library( - name = "gnss_util", - srcs = [], + name = "data_buffer", + srcs = [ + "data_buffer.cc", + ], + hdrs = [ + "data_buffer.h", + ], + deps = [ + ], +) + +cc_test( + name = "data_buffer_test", + srcs = [ + "data_buffer_test.cc", + ], + deps = [ + ":data_buffer", + "@googletest//:main", + ], +) + +cc_library( + name = "time_conversion", + hdrs = [ + "time_conversion.h", + ], + deps = [ + ], +) + +cc_library( + name = "util", + srcs = [ + ], hdrs = [ - "macros.h", + "util.h", "time_conversion.h", ], + deps = [ + ], +) + +cc_test( + name = "util_test", + srcs = [ + "util_test.cc", + ], + deps = [ + ":util", + "@googletest//:main", + ], ) cpplint() diff --git a/modules/drivers/gnss/util/data_buffer.cc b/modules/drivers/gnss/util/data_buffer.cc new file mode 100644 index 00000000000..9aaa50be4d4 --- /dev/null +++ b/modules/drivers/gnss/util/data_buffer.cc @@ -0,0 +1,110 @@ +// Copyright 2025 WheelOS All Rights Reserved. +// +// 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. + +// Created Date: 2025-4-15 +// Author: daohu527 + +#include "modules/drivers/gnss/util/data_buffer.h" + +namespace apollo { +namespace drivers { +namespace gnss { + +bool DataBuffer::SearchAndConsume(const std::string& pattern) { + std::lock_guard lock(mutex_); + auto it = std::search(buffer_.begin(), buffer_.end(), pattern.begin(), + pattern.end()); + if (it != buffer_.end()) { + buffer_.erase(buffer_.begin(), it + pattern.size()); + return true; + } + return false; +} + +bool DataBuffer::SearchAndConsume(const uint8_t* pattern, std::size_t length) { + std::lock_guard lock(mutex_); + auto it = + std::search(buffer_.begin(), buffer_.end(), pattern, pattern + length); + if (it != buffer_.end()) { + buffer_.erase(buffer_.begin(), it + length); + return true; + } + return false; +} + +// Renamed from Fetch, now clearly indicates reading and consuming +std::optional> DataBuffer::Read(std::size_t length) { + std::lock_guard lock(mutex_); + if (buffer_.size() < length) { + return std::nullopt; + } + + std::vector fetched_data; + fetched_data.reserve(length); + + std::copy(buffer_.begin(), buffer_.begin() + length, + std::back_inserter(fetched_data)); + + buffer_.erase(buffer_.begin(), buffer_.begin() + length); + + return fetched_data; +} + +bool DataBuffer::Consume(std::size_t length) { + std::lock_guard lock(mutex_); + if (length > buffer_.size()) { + return false; + } + + buffer_.erase(buffer_.begin(), buffer_.begin() + length); // 消费掉 + + return true; +} + +std::optional DataBuffer::Poll() { + std::lock_guard lock(mutex_); + if (buffer_.empty()) { + return std::nullopt; + } + uint8_t byte = buffer_.front(); + buffer_.pop_front(); + return byte; +} + +std::size_t DataBuffer::Size() const { + std::lock_guard lock(mutex_); + return buffer_.size(); +} + +bool DataBuffer::IsEmpty() const { + std::lock_guard lock(mutex_); + return buffer_.empty(); +} + +bool DataBuffer::CanAppend(std::size_t size_to_add) const { + std::lock_guard lock(mutex_); + // Be careful with potential unsigned integer overflow if size_to_add is very + // large A safer way to check: + if (size_to_add > capacity_) return false; + return buffer_.size() <= capacity_ - size_to_add; +} + +void DataBuffer::Clear() { + std::lock_guard lock(mutex_); + buffer_.clear(); +} + +} // namespace gnss +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/gnss/util/data_buffer.h b/modules/drivers/gnss/util/data_buffer.h new file mode 100644 index 00000000000..24bbadfaaa6 --- /dev/null +++ b/modules/drivers/gnss/util/data_buffer.h @@ -0,0 +1,88 @@ +// Copyright 2025 WheelOS All Rights Reserved. +// +// 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. + +// Created Date: 2025-4-15 +// Author: daohu527 + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "cyber/common/macros.h" + +namespace apollo { +namespace drivers { +namespace gnss { + +class DataBuffer { + public: + explicit DataBuffer(std::size_t capacity = 1024) : capacity_(capacity) {} + + ~DataBuffer() = default; + + bool Append(const std::string& data); + bool Append(const uint8_t* data, std::size_t length); + + // Searches for the pattern in the buffer. If found, removes all data from the + // front up to and including the pattern. Returns true if the pattern was + // found and consumed, false otherwise. Renamed from SearchAndFetch for clarity. + bool SearchAndConsume(const std::string& pattern); + bool SearchAndConsume(const uint8_t* pattern, std::size_t length); + + // Reads data from the front of the buffer and removes it. + // Returns a vector containing the first 'length' bytes, or std::nullopt + // if the buffer contains fewer than 'length' bytes. Renamed from Fetch. + std::optional> Read(std::size_t length); + + // Get the number of readable bytes currently in the buffer. + std::size_t Size() const; // Needs implementation with lock + + std::size_t Capacity() const { return capacity_; } + + // Check if the buffer is empty. + bool IsEmpty() const; // Needs implementation with lock + + // Check if data of the given size can be appended without exceeding capacity. + bool CanAppend(std::size_t size_to_add) const; // Needs implementation with lock + + // Consume (remove) data from the front of the buffer without returning it. + // Returns true if the full requested length was consumed, false otherwise + // (and consumes nothing). + bool Consume(std::size_t length); + + // Consume and return the first byte from the buffer. + // Returns std::nullopt if the buffer is empty. Behavior: Read 1 & Consume 1. + std::optional Poll(); + + void Clear(); + + private: + std::deque buffer_; + std::size_t capacity_; + mutable std::mutex mutex_; + + DISALLOW_COPY_AND_ASSIGN(DataBuffer); +}; + +} // namespace gnss +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/gnss/util/data_buffer_test.cc b/modules/drivers/gnss/util/data_buffer_test.cc new file mode 100644 index 00000000000..2eac646097d --- /dev/null +++ b/modules/drivers/gnss/util/data_buffer_test.cc @@ -0,0 +1,203 @@ +// Copyright 2025 WheelOS All Rights Reserved. +// +// 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. + +// Created Date: 2025-4-15 +// Author: daohu527 + +#include "modules/drivers/gnss/util/data_buffer.h" + +#include "gtest/gtest.h" + +namespace apollo { +namespace drivers { +namespace gnss { + +TEST(DataBufferTest, ConstructorDefaultCapacity) { + DataBuffer buffer; + EXPECT_EQ(buffer.Capacity(), 1024); + EXPECT_EQ(buffer.Size(), 0); + EXPECT_TRUE(buffer.IsEmpty()); +} + +TEST(DataBufferTest, ConstructorCustomCapacity) { + DataBuffer buffer(512); + EXPECT_EQ(buffer.Capacity(), 512); + EXPECT_EQ(buffer.Size(), 0); + EXPECT_TRUE(buffer.IsEmpty()); +} + +TEST(DataBufferTest, AppendString) { + DataBuffer buffer(10); + EXPECT_TRUE(buffer.Append("hello")); + EXPECT_EQ(buffer.Size(), 5); + EXPECT_FALSE(buffer.IsEmpty()); + EXPECT_TRUE(buffer.Append(" world")); + EXPECT_EQ(buffer.Size(), 10); + EXPECT_FALSE(buffer.Append("!")); // Capacity reached + EXPECT_EQ(buffer.Size(), 10); +} + +TEST(DataBufferTest, Capacity) { + DataBuffer buffer1; + EXPECT_EQ(buffer1.Capacity(), 1024); + DataBuffer buffer2(256); + EXPECT_EQ(buffer2.Capacity(), 256); +} + +TEST(DataBufferTest, IsEmpty) { + DataBuffer buffer; + EXPECT_TRUE(buffer.IsEmpty()); + buffer.Append("data"); + EXPECT_FALSE(buffer.IsEmpty()); + buffer.Fetch(new uint8_t[4], 4); + EXPECT_TRUE(buffer.IsEmpty()); +} + +TEST(DataBufferTest, CanAppend) { + DataBuffer buffer(10); + EXPECT_TRUE(buffer.CanAppend(5)); + EXPECT_TRUE(buffer.CanAppend(10)); + buffer.Append("abcdef"); + EXPECT_TRUE(buffer.CanAppend(4)); + EXPECT_FALSE(buffer.CanAppend(5)); +} + +TEST(DataBufferTest, SearchAndFetchStringFound) { + DataBuffer buffer; + buffer.Append("prefixpatternsuffix"); + EXPECT_TRUE(buffer.SearchAndFetch("pattern")); + EXPECT_EQ(buffer.Size(), 6); + uint8_t output[6]; + EXPECT_TRUE(buffer.Fetch(output, 6)); + EXPECT_EQ(std::string(output, output + 6), "suffix"); +} + +TEST(DataBufferTest, SearchAndFetchStringNotFound) { + DataBuffer buffer; + buffer.Append("some data here"); + EXPECT_FALSE(buffer.SearchAndFetch("pattern")); + EXPECT_EQ(buffer.Size(), 14); +} + +TEST(DataBufferTest, SearchAndFetchStringEmptyPattern) { + DataBuffer buffer; + buffer.Append("test"); + EXPECT_TRUE(buffer.SearchAndFetch("")); // Should match at the beginning + EXPECT_EQ(buffer.Size(), 4); + uint8_t output[4]; + EXPECT_TRUE(buffer.Fetch(output, 4)); + EXPECT_EQ(std::string(output, output + 4), "test"); +} + +TEST(DataBufferTest, SearchAndFetchStringPatternAtBeginning) { + DataBuffer buffer; + buffer.Append("patternsuffix"); + EXPECT_TRUE(buffer.SearchAndFetch("pattern")); + EXPECT_EQ(buffer.Size(), 6); + uint8_t output[6]; + EXPECT_TRUE(buffer.Fetch(output, 6)); + EXPECT_EQ(std::string(output, output + 6), "suffix"); +} + +TEST(DataBufferTest, SearchAndFetchStringPatternAtEnd) { + DataBuffer buffer; + buffer.Append("prefixpattern"); + EXPECT_TRUE(buffer.SearchAndFetch("pattern")); + EXPECT_EQ(buffer.Size(), 0); +} + +TEST(DataBufferTest, SearchAndFetchByteArrayFound) { + DataBuffer buffer; + buffer.Append(std::vector{0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07}); + uint8_t pattern[] = {0x03, 0x04, 0x05}; + EXPECT_TRUE(buffer.SearchAndFetch(pattern, sizeof(pattern))); + EXPECT_EQ(buffer.Size(), 2); + uint8_t output[2]; + EXPECT_TRUE(buffer.Fetch(output, 2)); + EXPECT_EQ(output[0], 0x06); + EXPECT_EQ(output[1], 0x07); +} + +TEST(DataBufferTest, SearchAndFetchByteArrayNotFound) { + DataBuffer buffer; + buffer.Append(std::vector{0x01, 0x02, 0x03}); + uint8_t pattern[] = {0x04, 0x05}; + EXPECT_FALSE(buffer.SearchAndFetch(pattern, sizeof(pattern))); + EXPECT_EQ(buffer.Size(), 3); +} + +TEST(DataBufferTest, SearchAndFetchByteArrayEmptyPattern) { + DataBuffer buffer; + buffer.Append(std::vector{0x01, 0x02, 0x03}); + uint8_t pattern[] = {}; + EXPECT_TRUE(buffer.SearchAndFetch(pattern, sizeof(pattern))); + EXPECT_EQ(buffer.Size(), 3); + uint8_t output[3]; + EXPECT_TRUE(buffer.Fetch(output, 3)); + EXPECT_EQ(output[0], 0x01); + EXPECT_EQ(output[1], 0x02); + EXPECT_EQ(output[2], 0x03); +} + +TEST(DataBufferTest, SearchAndFetchByteArrayPatternAtBeginning) { + DataBuffer buffer; + buffer.Append(std::vector{0x01, 0x02, 0x03, 0x04}); + uint8_t pattern[] = {0x01, 0x02}; + EXPECT_TRUE(buffer.SearchAndFetch(pattern, sizeof(pattern))); + EXPECT_EQ(buffer.Size(), 2); + uint8_t output[2]; + EXPECT_TRUE(buffer.Fetch(output, 2)); + EXPECT_EQ(output[0], 0x03); + EXPECT_EQ(output[1], 0x04); +} + +TEST(DataBufferTest, SearchAndFetchByteArrayPatternAtEnd) { + DataBuffer buffer; + buffer.Append(std::vector{0x01, 0x02, 0x03}); + uint8_t pattern[] = {0x02, 0x03}; + EXPECT_TRUE(buffer.SearchAndFetch(pattern, sizeof(pattern))); + EXPECT_EQ(buffer.Size(), 0); +} + +TEST(DataBufferTest, FetchSuccess) { + DataBuffer buffer; + buffer.Append("abcdef"); + uint8_t output[3]; + EXPECT_TRUE(buffer.Fetch(output, 3)); + EXPECT_EQ(std::string(output, output + 3), "abc"); + EXPECT_EQ(buffer.Size(), 3); + uint8_t remaining_output[3]; + EXPECT_TRUE(buffer.Fetch(remaining_output, 3)); + EXPECT_EQ(std::string(remaining_output, remaining_output + 3), "def"); + EXPECT_TRUE(buffer.IsEmpty()); +} + +TEST(DataBufferTest, FetchNotEnoughData) { + DataBuffer buffer; + buffer.Append("abc"); + uint8_t output[4]; + EXPECT_FALSE(buffer.Fetch(output, 4)); + EXPECT_EQ(buffer.Size(), 3); +} + +TEST(DataBufferTest, FetchEmptyBuffer) { + DataBuffer buffer; + uint8_t output[1]; + EXPECT_FALSE(buffer.Fetch(output, 1)); + EXPECT_TRUE(buffer.IsEmpty()); +} + +} // namespace gnss +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/gnss/util/util.h b/modules/drivers/gnss/util/util.h new file mode 100644 index 00000000000..61f8b52305b --- /dev/null +++ b/modules/drivers/gnss/util/util.h @@ -0,0 +1,166 @@ +// Copyright 2025 WheelOS All Rights Reserved. +// +// 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. + +// Created Date: 2025-4-15 +// Author: daohu527 + +#pragma once + +#include +#include +#include +#include + +#include "gnss/util/macros.h" +#include "google/protobuf/message.h" + +// Anonymous namespace that contains helper constants and functions. +namespace { + +constexpr int kSecondsPerWeek = 60 * 60 * 24 * 7; +constexpr double kDegToRad = M_PI / 180.0; +constexpr float kFloatNaN = std::numeric_limits::quiet_NaN(); + +// The NovAtel's orientation covariance matrix is pitch, roll, and yaw. We use +// the index array below to convert it to the orientation covariance matrix with +// order roll, pitch, and yaw. +constexpr int INDEX[] = {4, 3, 5, 1, 0, 2, 7, 6, 8}; +static_assert(sizeof(INDEX) == 9 * sizeof(int), "Incorrect size of INDEX"); + +template +constexpr bool is_zero(T value) { + static_assert( + std::is_floating_point_v || std::is_integral_v, + "is_zero can only be used with floating point or integral types"); + + if constexpr (std::is_floating_point_v) { + return std::abs(value) < std::numeric_limits::epsilon(); + } else { + return value == T{0}; + } +} + +// CRC algorithm from the NovAtel document. +inline uint32_t crc32_word(uint32_t word) { + for (int j = 0; j < 8; ++j) { + if (word & 1) { + word = (word >> 1) ^ 0xEDB88320; + } else { + word >>= 1; + } + } + return word; +} + +inline uint32_t crc32_block(const uint8_t *buffer, size_t length) { + uint32_t word = 0; + while (length--) { + uint32_t t1 = (word >> 8) & 0xFFFFFF; + uint32_t t2 = crc32_word((word ^ *buffer++) & 0xFF); + word = t1 ^ t2; + } + return word; +} + +// Converts NovAtel's azimuth (north = 0, east = 90) to FLU yaw (east = 0, north +// = pi/2). +constexpr double azimuth_deg_to_yaw_rad(double azimuth) { + return (90.0 - azimuth) * kDegToRad; +} + +// A helper that fills an Point3D object (which uses the FLU frame) using RFU +// measurements. +inline void rfu_to_flu(double r, double f, double u, + ::apollo::common::Point3D *flu) { + flu->set_x(f); + flu->set_y(-r); + flu->set_z(u); +} + +uint8_t str_to_uint8(const std::string &str) { + try { + unsigned long num = std::stoul(str); + + if (num > 255) { + throw std::out_of_range("Number out of uint8_t range"); + } + + return static_cast(num); + } catch (const std::invalid_argument &e) { + throw std::invalid_argument("Invalid number format"); + } catch (const std::out_of_range &e) { + throw std::out_of_range("Number out of uint8_t range"); + } +} + +void str_to_char_array(const std::string &src, char *dest) { + strncpy(dest, src.c_str(), size_of(src)); +} + +} // namespace + +namespace apollo { +namespace drivers { +namespace gnss { + +constexpr char NMEA_START_FLAG = '$'; // Start flag +constexpr char NMEA_FIELD_SEPARATOR = ','; // Field separator +constexpr char NMEA_CHECKSUM_SEPARATOR = '*'; // Checksum separator +constexpr char NMEA_END_OF_LINE_CR = '\r'; // End of line: Carriage return +constexpr char NMEA_END_OF_LINE_LF = '\n'; // End of line: Line feed + +// convert gps time (base on Jan 6 1980) to system time (base on Jan 1 1970) +// notice: Jan 6 1980 +// +// linux shell: +// time1 = date +%s -d"Jan 6, 1980 00:00:01" +// time2 = date +%s -d"Jan 1, 1970 00:00:01" +// dif_tick = time1-time2 +// 315964800 = 315993601 - 28801 + +constexpr long long kEpochAndSystemDiffSeconds = 315964800LL; + +// A helper function that returns a pointer to a protobuf message of type T. +template +inline T *As(::google::protobuf::Message *message_ptr) { + static_assert(std::is_base_of_v<::google::protobuf::Message, T>, + "T must be derived from ::google::protobuf::Message"); + return dynamic_cast(message_ptr); +} + +std::vector ExtractAndSplitPayload( + const std::vector &frame_bytes, std::size_t crc_length, + char field_separator) { + std::vector payload_fields; + if (frame_bytes.size() <= crc_length) { + return payload_fields; + } + + auto payload_begin = frame_bytes.begin(); + auto payload_end = frame_bytes.end() - crcLength; + + std::string payload(payload_begin, payload_end); + + std::stringstream payload_stream(payload); + std::string field; + while (std::getline(payload_stream, field, field_separator)) { + payload_fields.push_back(field); + } + + return payload_fields; +} + +} // namespace gnss +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/gnss/util/util_test.cc b/modules/drivers/gnss/util/util_test.cc new file mode 100644 index 00000000000..cf671fdd18c --- /dev/null +++ b/modules/drivers/gnss/util/util_test.cc @@ -0,0 +1,144 @@ +// Copyright 2025 WheelOS All Rights Reserved. +// +// 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. + +// Created Date: 2025-4-15 +// Author: daohu527 + +#include "gnss/util/util.h" + +#include "gtest/gtest.h" + +namespace apollo { +namespace drivers { +namespace gnss { + +TEST(CommonTest, IsZero) { + EXPECT_TRUE(is_zero(0)); + EXPECT_TRUE(is_zero(0.0f)); + EXPECT_TRUE(is_zero(0.0)); + EXPECT_TRUE(is_zero(static_cast(0))); + EXPECT_FALSE(is_zero(1)); + EXPECT_FALSE(is_zero(-1.0f)); + EXPECT_FALSE(is_zero(0.0001f)); // Should be false for standard epsilon + EXPECT_FALSE(is_zero(-0.0000001)); +} + +TEST(CommonTest, Crc32Word) { + EXPECT_EQ(crc32_word(0), 0); + EXPECT_EQ(crc32_word(1), 0xEDB88320); + EXPECT_EQ(crc32_word(0xFFFFFFFF), 0xC9E95ECB); +} + +TEST(CommonTest, Crc32Block) { + std::vector empty_buffer = {}; + EXPECT_EQ(crc32_block(empty_buffer.data(), empty_buffer.size()), 0); + + std::vector single_byte_buffer = {0x01}; + EXPECT_EQ(crc32_block(single_byte_buffer.data(), single_byte_buffer.size()), + 0xEDB88320); + + std::vector multi_byte_buffer = {0x12, 0x34, 0x56, 0x78}; + EXPECT_EQ(crc32_block(multi_byte_buffer.data(), multi_byte_buffer.size()), + 0x87D6E60C); +} + +TEST(CommonTest, AzimuthDegToYawRad) { + EXPECT_TRUE(FloatEq(azimuth_deg_to_yaw_rad(0.0), M_PI / 2.0)); + EXPECT_TRUE(FloatEq(azimuth_deg_to_yaw_rad(90.0), 0.0)); + EXPECT_TRUE(FloatEq(azimuth_deg_to_yaw_rad(180.0), -M_PI / 2.0)); + EXPECT_TRUE(FloatEq(azimuth_deg_to_yaw_rad(270.0), -M_PI)); + EXPECT_TRUE(FloatEq(azimuth_deg_to_yaw_rad(360.0), M_PI / 2.0)); + EXPECT_TRUE(FloatEq(azimuth_deg_to_yaw_rad(45.0), M_PI * 0.25)); +} + +TEST(CommonTest, RfuToFlu) { + apollo::common::Point3D flu; + rfu_to_flu(1.0, 2.0, 3.0, &flu); + EXPECT_TRUE(FloatEq(flu.x(), 2.0)); + EXPECT_TRUE(FloatEq(flu.y(), -1.0)); + EXPECT_TRUE(FloatEq(flu.z(), 3.0)); + + rfu_to_flu(-1.0, 0.5, -2.0, &flu); + EXPECT_TRUE(FloatEq(flu.x(), 0.5)); + EXPECT_TRUE(FloatEq(flu.y(), 1.0)); + EXPECT_TRUE(FloatEq(flu.z(), -2.0)); + + rfu_to_flu(0.0, 0.0, 0.0, &flu); + EXPECT_TRUE(FloatEq(flu.x(), 0.0)); + EXPECT_TRUE(FloatEq(flu.y(), 0.0)); + EXPECT_TRUE(FloatEq(flu.z(), 0.0)); +} + +TEST(CommonTest, StrToUint8) { + EXPECT_EQ(str_to_uint8("0"), 0); + EXPECT_EQ(str_to_uint8("255"), 255); + EXPECT_EQ(str_to_uint8("128"), 128); + + EXPECT_THROW(str_to_uint8("256"), std::out_of_range); + EXPECT_THROW(str_to_uint8("-1"), std::out_of_range); + EXPECT_THROW(str_to_uint8("abc"), std::invalid_argument); + EXPECT_THROW(str_to_uint8("12.3"), std::invalid_argument); + EXPECT_THROW(str_to_uint8(""), std::invalid_argument); +} + +TEST(CommonTest, StrToCharArray) { + char buffer[10]; + + str_to_char_array("hello", buffer, sizeof(buffer)); + EXPECT_STREQ(buffer, "hello"); + + str_to_char_array("world12345", buffer, sizeof(buffer)); + EXPECT_STREQ( + buffer, + "world1234"); // Should truncate, leaving space for null terminator + + str_to_char_array("", buffer, sizeof(buffer)); + EXPECT_STREQ(buffer, ""); + + char small_buffer[3]; + str_to_char_array("ab", small_buffer, sizeof(small_buffer)); + EXPECT_STREQ(small_buffer, "ab"); + + str_to_char_array("abc", small_buffer, sizeof(small_buffer)); + EXPECT_STREQ(small_buffer, "ab"); // Should truncate + + char one_byte_buffer[1]; + str_to_char_array("", one_byte_buffer, sizeof(one_byte_buffer)); + EXPECT_STREQ(one_byte_buffer, ""); +} + +TEST(Util, ExtractAndSplitPayload) { + std::string payload = "1234567890"; + std::vector frame_data(payload.begin(), payload.end()); + std::size_t crc_length = 3; + std::vector items = + ExtractAndSplitPayload(frame_data, crc_length, ','); + ASSERT_EQ(items.size(), 1); + EXPECT_EQ(items[0], "1234567"); + std::string payload_with_crc = "12,34,56,78,90*AB"; + std::vector frame_data_with_crc(payload_with_crc.begin(), + payload_with_crc.end()); + std::vector items_with_crc = + ExtractAndSplitPayload(frame_data_with_crc, crc_length, ','); + ASSERT_EQ(items_with_crc.size(), 5); + EXPECT_EQ(items_with_crc[0], "12"); + EXPECT_EQ(items_with_crc[0], "34"); + EXPECT_EQ(items_with_crc[0], "56"); + EXPECT_EQ(items_with_crc[0], "78"); + EXPECT_EQ(items_with_crc[0], "90"); +} + +} // namespace gnss +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/hal/stream/BUILD b/modules/drivers/hal/stream/BUILD new file mode 100644 index 00000000000..59c86a4c54c --- /dev/null +++ b/modules/drivers/hal/stream/BUILD @@ -0,0 +1,93 @@ +load("@rules_cc//cc:defs.bzl", "cc_library", "cc_test") +load("//tools:cpplint.bzl", "cpplint") + +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "stream", + hdrs = ["stream.h"], + deps = [ + "//cyber", + ], +) + +cc_library( + name = "can_stream", + srcs = ["can_stream.cc"], + hdrs = ["can_stream.h"], + deps = [ + ":stream", + "//cyber", + ], +) + +cc_library( + name = "ntrip_stream", + srcs = ["ntrip_stream.cc"], + hdrs = ["ntrip_stream.h"], + deps = [ + ":stream", + ":tcp_stream", + "//cyber", + "//modules/common/util", + ], +) + +cc_library( + name = "serial_stream", + srcs = ["serial_stream.cc"], + hdrs = ["serial_stream.h"], + deps = [ + ":stream", + "//cyber", + ], +) + +cc_library( + name = "tcp_stream", + srcs = ["tcp_stream.cc"], + hdrs = ["tcp_stream.h"], + deps = [ + ":stream", + "//cyber", + ], +) + +cc_library( + name = "udp_stream", + srcs = ["udp_stream.cc"], + hdrs = ["udp_stream.h"], + deps = [ + ":stream", + "//cyber", + ], +) + +cc_library( + name = "stream_factory", + srcs = ["stream_factory.cc"], + hdrs = ["stream_factory.h"], + deps = [ + ":stream", + ":can_stream", + ":ntrip_stream", + ":serial_stream", + ":tcp_stream", + ":udp_stream", + "//cyber", + ], + alwayslink = 1, +) + +# TODO(zero): recover the test code for stream +# cc_test( +# name = "can_stream_test", +# srcs = ["can_stream_test.cc"], +# deps = [ +# ":can_stream", +# "@com_google_googletest//:gtest", +# "@com_google_googletest//:gtest_main", +# ], +# ) + +cpplint() diff --git a/modules/drivers/hal/stream/can_stream.cc b/modules/drivers/hal/stream/can_stream.cc new file mode 100644 index 00000000000..1d03ce8b880 --- /dev/null +++ b/modules/drivers/hal/stream/can_stream.cc @@ -0,0 +1,407 @@ +// Copyright 2025 WheelOS. All Rights Reserved. +// +// 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. + +// Created Date: 2025-03-21 +// Author: daohu527 + +#include "modules/drivers/hal/stream/can_stream.h" + +#include "cyber/cyber.h" + +namespace apollo { +namespace drivers { +namespace hal { + +// Constructor: Stores interface name, timeout, and CAN FD flag. Doesn't open +// the socket. Throws std::invalid_argument if interface name is empty or too +// long. +CanStream::CanStream(const std::string &address, uint32_t timeout_usec, + bool enable_can_fd) + : interface_name_(address), + timeout_usec_(timeout_usec), + fd_(-1), + enable_can_fd_(enable_can_fd) { + if (interface_name_.empty()) { + throw std::invalid_argument("CAN interface name cannot be empty."); + } + + if (interface_name_.length() >= IFNAMSIZ) { + throw std::invalid_argument("CAN interface name '" + interface_name_ + + "' is too long."); + } + // Port is unused for basic CAN_RAW, ignore it. + // Socket is not opened until Connect() is called. + // status_ is assumed DISCONNECTED initially by the base class constructor. + AINFO << "CanStream created for interface: " << interface_name_ + << ", timeout: " << timeout_usec << " us" + << ", CAN FD enabled: " << (enable_can_fd ? "true" : "false"); +} + +// Destructor: Ensures the CAN socket is closed. +CanStream::~CanStream() { + this->close(); // Safe to call multiple times +} + +// Helper function to create, bind, and configure the CAN socket. +// Throws std::runtime_error on failure. +void CanStream::open() { + if (fd_ >= 0) { + // Already open + return; + } + + // 1. Create the socket + int fd = socket(AF_CAN, SOCK_RAW, CAN_RAW); + if (fd < 0) { + last_errno_ = errno; + throw std::runtime_error("Failed to create CAN socket: " + + std::string(strerror(last_errno_))); + } + + // 2. Enable CAN FD support if requested + if (enable_can_fd_) { + int enable_fd = 1; + if (setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_fd, + sizeof(enable_fd)) < 0) { + last_errno_ = errno; + ::close(fd); + throw std::runtime_error("Failed to enable CAN FD frames on socket: " + + std::string(strerror(last_errno_))); + } + AINFO << "CAN FD frames enabled on socket."; + } + + // 3. Get the interface index + struct ifreq ifr; + std::memset(&ifr, 0, sizeof(ifr)); + std::strncpy(ifr.ifr_name, interface_name_.c_str(), IFNAMSIZ - 1); + ifr.ifr_name[IFNAMSIZ - 1] = '\0'; // Ensure null termination + + if (ioctl(fd, SIOCGIFINDEX, &ifr) < 0) { + last_errno_ = errno; + ::close(fd); + throw std::runtime_error("Failed to get CAN interface index for '" + + interface_name_ + + "': " + std::string(strerror(last_errno_))); + } + + // 4. Bind the socket to the interface index + struct sockaddr_can addr; + std::memset(&addr, 0, sizeof(addr)); + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (bind(fd, reinterpret_cast(&addr), sizeof(addr)) < 0) { + last_errno_ = errno; + ::close(fd); // Clean up socket on error + throw std::runtime_error("Failed to bind CAN socket to interface index " + + std::to_string(addr.can_ifindex) + ": " + + std::string(strerror(last_errno_))); + } + + // 5. Set read/write timeouts (optional, based on timeout_usec_) + if (timeout_usec_ > 0) { + struct timeval tv = { + static_cast(timeout_usec_ / 1000000), // seconds + static_cast(timeout_usec_ % 1000000) // microseconds + }; + + if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)) < 0) { + last_errno_ = errno; + ::close(fd); + throw std::runtime_error("Failed to set SO_RCVTIMEO on CAN socket: " + + std::string(strerror(last_errno_))); + } + + if (setsockopt(fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv)) < 0) { + last_errno_ = errno; + ::close(fd); + throw std::runtime_error("Failed to set SO_SNDTIMEO on CAN socket: " + + std::string(strerror(last_errno_))); + } + } + // TODO(zero): If timeout_usec_ == 0, read/write will be non-blocking + // (EAGAIN/EWOULDBLOCK on no data/buffer). + + // Socket is successfully created, bound, and configured + fd_ = fd; + last_errno_ = 0; + status_ = Stream::Status::CONNECTED; + AINFO << "Successfully opened CAN socket for interface: " << interface_name_ + << ", fd: " << fd_; +} + +// Helper function to close the CAN socket. Safe to call multiple times. +void CanStream::close() { + if (fd_ >= 0) { // Check if socket is open + AINFO << "Closing CAN socket for interface " << interface_name_ + << ", fd: " << fd_; + if (::close(fd_) < 0) { + last_errno_ = errno; + AERROR << "Failed to close CAN socket fd " << fd_ << ": " + << strerror(last_errno_); + // Don't throw on close failure in destructor or Disconnect. + } + fd_ = -1; + } + + status_ = Stream::Status::DISCONNECTED; +} + +// Connect: Creates, binds, and configures the CAN socket. +// Returns true if successful or already connected. +// Returns false on connection failure (when not throwing). +// Throws std::runtime_error for fatal setup errors. +bool CanStream::Connect() { + if (fd_ >= 0) { + return true; + } + + last_errno_ = 0; + + try { + // open() handles socket creation, binding, and configuration. + // It throws std::runtime_error on failure. + this->open(); + AINFO << "CanStream connected successfully to interface: " + << interface_name_ << ", fd: " << fd_; + return true; + } catch (const std::exception &e) { + last_errno_ = (fd_ < 0 && errno != 0) ? errno : 0; + AERROR << "Failed to Connect to CAN interface " << interface_name_ << ": " + << e.what(); + // fd_ is already -1 if open failed before setting it, or set to -1 by + // open's cleanup on config failure. + status_ = Stream::Status::ERROR; + } + return false; // Connection failed +} + +// Disconnect: Closes the CAN socket. +bool CanStream::Disconnect() { + if (fd_ < 0) { + AINFO << "CanStream already disconnected for interface: " + << interface_name_; + return false; + } + + AINFO << "Disconnecting CAN stream for interface " << interface_name_ + << ", fd: " << fd_; + this->close(); // close() sets fd_ = -1 and updates status_ + last_errno_ = 0; + return true; +} + +// Reads CAN frames into the buffer. +// buffer must be large enough to hold at least one frame (sizeof(struct +// can_frame) or sizeof(struct canfd_frame)). max_length should be a multiple of +// the chosen frame size. Returns number of bytes read (multiple of frame size, +// >= 0). Returns 0 on timeout or non-blocking with no data +// (EAGAIN/EWOULDBLOCK). Throws std::runtime_error on fatal read errors or +// connection loss. +size_t CanStream::read(uint8_t *buffer, size_t max_length) { + if (fd_ < 0) { + last_errno_ = ENOTCONN; + AERROR << "CanStream read failed: Not connected."; + return 0; + } + + if (buffer == nullptr || max_length == 0) { + return 0; + } + + // Determine frame size based on enabled mode + const size_t frame_size = get_frame_size(); + + // Ensure max_length is a multiple of frame size + if (max_length < frame_size || (max_length % frame_size) != 0) { + last_errno_ = EINVAL; // Invalid argument + AERROR << "CanStream read failed: max_length (" << max_length + << ") must be a multiple of CAN frame size (" << frame_size << ")."; + throw std::invalid_argument( + "max_length must be a multiple of CAN frame size."); + } + + // Calculate how many frames can fit + size_t max_frames = max_length / frame_size; + size_t frames_read = 0; + uint8_t *current_buffer_pos = buffer; + + // Read loop to fill the buffer with as many frames as possible (up to + // max_frames) recv respects the timeout_usec_ set via SO_RCVTIMEO. + // TODO(zero): non-blocking (timeout_usec_ == 0), recv returns + // EAGAIN/EWOULDBLOCK immediately. + while (frames_read < max_frames) { + ssize_t bytes_received; + // Use recv specifically for sockets. + do { + bytes_received = recv(fd_, current_buffer_pos, frame_size, 0); + } while (bytes_received < 0 && errno == EINTR); // Handle EINTR + + if (bytes_received < 0) { + // Error during recv + if (errno == EAGAIN || errno == EWOULDBLOCK) { + // Timeout or non-blocking socket has no data available immediately. + // This is not a fatal error; break the loop and return frames read so + // far. + last_errno_ = errno; // Store EAGAIN/EWOULDBLOCK + break; // Exit while loop + } else { + // Other serious errors (e.g., socket closed, interface down, + // permissions) + last_errno_ = errno; + AERROR << "CanStream read failed: " << strerror(last_errno_) + << " (errno: " << last_errno_ << "), fd: " << fd_ + << ", interface: " << interface_name_; + status_ = Stream::Status::ERROR; + Disconnect(); + throw std::runtime_error("CanStream read fatal error: " + + std::string(strerror(last_errno_))); + } + } else if (bytes_received == 0) { + // recv returned 0. For sockets, this usually indicates the peer has + // performed an orderly shutdown. For CAN, this might indicate the + // interface went down or was closed. Treat as connection lost. + last_errno_ = ENOTCONN; // Use ENOTCONN (Not connected) + AERROR << "CanStream read failed: Connection lost (recv returned 0). FD: " + << fd_ << ", interface: " << interface_name_; + status_ = Stream::Status::DISCONNECTED; + Disconnect(); // Clean up + throw std::runtime_error("CanStream read connection lost."); + } else { // bytes_received > 0 + // Successfully read data. Should be equal to frame_size for CAN_RAW. + if (static_cast(bytes_received) != frame_size) { + // Received less than a full frame. This is unexpected for CAN_RAW recv. + last_errno_ = EIO; // I/O error + AERROR << "CanStream read failed: Received partial CAN frame. Expected " + << frame_size << " bytes, got " << bytes_received + << ". FD: " << fd_ << ", interface: " << interface_name_; + // This indicates a serious issue. Disconnect and throw. + Disconnect(); + throw std::runtime_error( + "CanStream read fatal error: Received partial frame."); + } + // Successfully read one frame + frames_read++; + current_buffer_pos += frame_size; + last_errno_ = 0; // Clear error on success + // Loop continues to try reading the next frame if space allows. + } + } + + // Return total bytes read across all frames + return frames_read * frame_size; +} + +// Writes CAN frames from the data buffer. +// length must be a multiple of the chosen frame size. +// Returns number of bytes written (multiple of frame size, >= 0). +// Returns 0 if not connected or on non-blocking EAGAIN/timeout. +// Throws std::runtime_error on fatal write errors. +size_t CanStream::write(const uint8_t *data, size_t length) { + if (fd_ < 0) { + last_errno_ = ENOTCONN; + AERROR << "CanStream write failed: Not connected."; + return 0; + } + + if (data == nullptr || length == 0) { + return 0; + } + + // Determine frame size based on enabled mode + const size_t frame_size = get_frame_size(); + + // Ensure length is a multiple of frame size + if (length < frame_size || (length % frame_size) != 0) { + last_errno_ = EINVAL; // Invalid argument + AERROR << "CanStream write failed: length (" << length + << ") must be a multiple of CAN frame size (" << frame_size << ")."; + throw std::invalid_argument("length must be a multiple of CAN frame size."); + } + + // Calculate how many frames to send + size_t num_frames = length / frame_size; + size_t frames_sent = 0; + const uint8_t *current_data_pos = data; + + // Write loop to send all frames from the buffer + // send respects the timeout_usec_ set via SO_SNDTIMEO. + // In non-blocking (timeout_usec_ == 0), send returns EAGAIN/EWOULDBLOCK + // immediately if buffer is full. + while (frames_sent < num_frames) { + ssize_t bytes_sent; + do { + bytes_sent = send(fd_, current_data_pos, frame_size, 0); + } while (bytes_sent < 0 && errno == EINTR); // Handle EINTR + + if (bytes_sent < 0) { + // Error during send + if (errno == EAGAIN || errno == EWOULDBLOCK) { + // Kernel buffer is full or timeout occurred before sending the frame. + // This is not a fatal error; break the loop and return frames sent so + // far. + last_errno_ = errno; // Store EAGAIN/EWOULDBLOCK + break; // Exit while loop + } else { + // Other serious errors (e.g., socket closed, interface down, + // permissions) + last_errno_ = errno; + AERROR << "CanStream write failed: " << strerror(last_errno_) + << " (errno: " << last_errno_ << "), fd: " << fd_ + << ", interface: " << interface_name_; + status_ = Stream::Status::ERROR; + // Fatal error. Disconnect and throw. + Disconnect(); + throw std::runtime_error("CanStream write fatal error: " + + std::string(strerror(last_errno_))); + } + } else if (bytes_sent == 0) { + // send returned 0. This is unusual for send on a healthy socket unless + // length was 0. With frame_size > 0, 0 bytes sent with no error usually + // indicates a problem. Treat as fatal error. + last_errno_ = EIO; // I/O error or similar connection issue + AERROR << "CanStream write failed: Sent 0 bytes unexpectedly. FD: " << fd_ + << ", interface: " << interface_name_; + status_ = Stream::Status::ERROR; + Disconnect(); + throw std::runtime_error( + "CanStream write fatal error: Sent 0 bytes unexpectedly."); + } else { // bytes_sent > 0 + // Successfully sent data. Should be equal to frame_size for CAN_RAW. + if (static_cast(bytes_sent) != frame_size) { + // Sent less than a full frame. This is unexpected for CAN_RAW send. + last_errno_ = EIO; // I/O error + AERROR << "CanStream write failed: Sent partial CAN frame. Expected " + << frame_size << " bytes, got " << bytes_sent << ". FD: " << fd_ + << ", interface: " << interface_name_; + // This indicates a serious issue. Disconnect and throw. + Disconnect(); + throw std::runtime_error( + "CanStream write fatal error: Sent partial frame."); + } + // Successfully sent one frame + frames_sent++; + current_data_pos += frame_size; + last_errno_ = 0; // Clear error on success + // Loop continues to try sending the next frame if space allows. + } + } + + return frames_sent * frame_size; +} + +} // namespace hal +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/hal/stream/can_stream.h b/modules/drivers/hal/stream/can_stream.h new file mode 100644 index 00000000000..eb8082f2ec8 --- /dev/null +++ b/modules/drivers/hal/stream/can_stream.h @@ -0,0 +1,111 @@ +// Copyright 2025 WheelOS. All Rights Reserved. +// +// 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. + +// Created Date: 2025-03-21 +// Author: daohu527 + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "modules/drivers/hal/stream/stream.h" + +namespace apollo { +namespace drivers { +namespace hal { + +// Implements a Stream interface for CAN bus communication using Linux +// SocketCAN. Reads and writes operate on raw struct can_frame or struct +// canfd_frame structures depending on the enable_can_fd flag passed in the +// constructor. max_length/length must be a multiple of the chosen frame size. +class CanStream : public Stream { + public: + // Constructor takes CAN interface name (address), port (unused for basic + // SocketCAN), timeout, and a flag to enable CAN FD support. Throws + // std::invalid_argument if interface name is empty or too long. + CanStream(const std::string &address, uint32_t timeout_usec, + bool enable_can_fd = false); + + // Destructor closes the CAN socket. + ~CanStream() override; + + // Connect: Creates, binds, and configures the CAN socket. + // Returns true if socket is successfully opened/already open. + // Returns false on failure. + bool Connect() override; + + // Disconnect: Closes the CAN socket. + // Returns true if socket is successfully closed/already closed. + bool Disconnect() override; + + // Reads CAN frames into the buffer. + // buffer must be large enough to hold at least one frame (sizeof(struct + // can_frame) or sizeof(struct canfd_frame)). max_length should be a multiple + // of the chosen frame size. Returns number of bytes read (multiple of frame + // size, >= 0). Returns 0 on timeout or non-blocking with no data + // (EAGAIN/EWOULDBLOCK). Throws std::runtime_error on fatal read errors or + // connection loss. + size_t read(uint8_t *buffer, size_t max_length) override; + + // Writes CAN frames from the data buffer. + // data buffer must contain one or more frame structures. + // length must be a multiple of the chosen frame size. + // Returns number of bytes written (multiple of frame size, >= 0). + // Returns 0 if not connected or on non-blocking EAGAIN/timeout. + // Throws std::runtime_error on fatal write errors. + size_t write(const uint8_t *data, size_t length) override; + + private: + // Helper function to create, bind, and configure the CAN socket. + // Throws std::runtime_error on failure. + void open(); + + // Helper function to close the CAN socket. Safe to call multiple times. + void close(); + + // Determines the size of the frame structure based on enable_can_fd_. + size_t get_frame_size() const { + return enable_can_fd_ ? sizeof(struct canfd_frame) + : sizeof(struct can_frame); + } + + private: + // CAN interface name (e.g., "can0") + std::string interface_name_; + + // Read/Write timeout in microseconds. 0 indicates non-blocking. + uint32_t timeout_usec_ = 0; + + // File descriptor for the CAN socket. -1 indicates not open. + int fd_ = -1; + + // Flag indicating if CAN FD support is enabled for this instance. + bool enable_can_fd_ = false; +}; + +} // namespace hal +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/hal/stream/can_stream_test.cc b/modules/drivers/hal/stream/can_stream_test.cc new file mode 100644 index 00000000000..e666579b6d3 --- /dev/null +++ b/modules/drivers/hal/stream/can_stream_test.cc @@ -0,0 +1,949 @@ +// Copyright 2025 WheelOS. All Rights Reserved. +// +// 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. + +// Created Date: 2025-03-21 +// Author: daohu527 + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +// --- Mock CanStream for Testing --- +// We mock CanStream to control what its read() and write() methods do, +// simulating interaction with a CAN interface without touching real hardware. +class MockCanStream : public apollo::drivers::hal::CanStream { + public: + // Pass parameters to base constructor. enable_can_fd is crucial for testing + // different modes. + MockCanStream(const std::string& address, uint16_t port, + uint32_t timeout_usec, bool enable_can_fd) + : CanStream(address, port, timeout_usec, enable_can_fd) {} + + // Mock virtual methods we interact with + // Note: We are NOT mocking Connect/Disconnect to test them, but rather + // to control the *state* of the mock (connected/disconnected) for read/write + // tests. If you needed to test CanStream::Connect/Disconnect implementation + // logic, you'd need a partial mock or OS interaction test setup. + MOCK_METHOD(bool, Connect, (), (override)); + MOCK_METHOD(bool, Disconnect, (), (override)); + + MOCK_METHOD(size_t, read, (uint8_t* buffer, size_t max_length), (override)); + MOCK_METHOD(size_t, write, (const uint8_t* data, size_t length), (override)); + + // We might need to mock get_last_error_code and get_status if the code uses + // them heavily MOCK_METHOD(int, get_last_error_code, (), (const, override)); + // MOCK_METHOD(Stream::Status, get_status, (), (const, override)); + + // --- Helper methods for simulating read/write data --- + + // Helper to simulate data available for reading by setting an internal + // buffer. The test sets this buffer with raw bytes representing CAN frames. + void set_read_data(const std::vector& data) { + read_buffer_ = data; + read_buffer_pos_ = 0; + } + + // Helper to get the data that was "written" to the mock. + const std::vector& get_written_data() const { return written_data_; } + + // Action method for mock read: copies data from internal buffer. + // Simulates reading available bytes up to max_length. + // This action is used for both Standard and FD tests; the size difference is + // handled by max_length. + size_t MockReadAction(uint8_t* buffer, size_t max_length) { + size_t bytes_available = read_buffer_.size() - read_buffer_pos_; + size_t bytes_to_copy = std::min(max_length, bytes_available); + + if (bytes_to_copy > 0) { + std::memcpy(buffer, read_buffer_.data() + read_buffer_pos_, + bytes_to_copy); + read_buffer_pos_ += bytes_to_copy; + } else { + // Simulate non-blocking/timeout behavior: if no data, return 0. + // For real error simulation (EAGAIN, ETIMEDOUT), you'd combine this with + // .WillRepeatedly(::testing::Return(-1)) and + // .WillRepeatedly(::testing::SetErrno(EAGAIN)) + } + + return bytes_to_copy; + } + + // Action method for mock write: appends data to an internal buffer. + // Simulates successful write of all requested bytes. + // This action is used for both Standard and FD tests; the size difference is + // handled by length. + size_t MockWriteAction(const uint8_t* data, size_t length) { + written_data_.insert(written_data_.end(), data, data + length); + return length; + // For simulating partial write or errors, use + // .WillOnce(Return(partial_size)) or .WillOnce(Throw(...)) etc. + } + + private: + std::vector read_buffer_; + size_t read_buffer_pos_ = 0; + std::vector written_data_; +}; + +// --- Gtest Test Suite for CAN Parsing Logic (Upper Layer) --- +// These tests focus *only* on how the upper layer code should interpret +// the raw bytes received from Stream::read as CAN frames. +// They DO NOT test CanStream::read itself. +TEST(CanStreamParsingTest, ParseStandardFrame_Single_ValidDLC) { + // This test demonstrates how upper layer parses bytes into a standard CAN + // frame. + using namespace apollo::drivers::hal; + + // 1. Manually create the byte representation of a standard CAN frame + // This simulates the bytes received by Stream::read into a buffer + struct can_frame raw_frame_bytes; + raw_frame_bytes.can_id = 0x123; // Example CAN ID + raw_frame_bytes.can_dlc = 5; // Example data length code (5 bytes) + raw_frame_bytes.data[0] = 0xAA; + raw_frame_bytes.data[1] = 0xBB; + raw_frame_bytes.data[2] = 0xCC; + raw_frame_bytes.data[3] = 0xDD; + raw_frame_bytes.data[4] = 0xEE; + // Bytes 5,6,7 in the data array are conceptually invalid according to DLC=5 + raw_frame_bytes.data[5] = 0xFF; // Garbage byte example + raw_frame_bytes.data[6] = 0x00; + raw_frame_bytes.data[7] = 0x11; + + // Prepare the byte buffer as received by Stream::read + const size_t frame_size = sizeof(struct can_frame); // e.g., 16 bytes + std::vector received_bytes(frame_size); + std::memcpy(received_bytes.data(), &raw_frame_bytes, frame_size); + + // --- Upper Layer Parsing Logic (Code under test in this specific test) --- + + // Assume 'buffer' points to received_bytes.data() and 'bytes_read' == + // received_bytes.size() + uint8_t* buffer = received_bytes.data(); + size_t bytes_read = received_bytes.size(); + + // 1. Upper layer must know the expected frame size (Standard CAN in this + // case) + const size_t expected_frame_size = sizeof(struct can_frame); + + // 2. Basic check: enough bytes for at least one frame, and total bytes is + // multiple of frame size + ASSERT_GE(bytes_read, expected_frame_size); + ASSERT_EQ(bytes_read % expected_frame_size, 0); + + // 3. Reinterpret the bytes as a CAN frame structure + // This is the core parsing step at this level + const struct can_frame* parsed_frame = + reinterpret_cast(buffer); + + // 4. Access and verify frame fields + ASSERT_EQ(parsed_frame->can_id, 0x123); + ASSERT_EQ(parsed_frame->can_dlc, 5); + + // 5. Access and verify data payload (only up to DLC) + // The upper layer uses can_dlc to know how many bytes in data[] are valid + ASSERT_EQ(parsed_frame->can_dlc, 5); + // Verify the first 5 bytes of data[] match the expected data + ASSERT_TRUE(std::memcmp(parsed_frame->data, raw_frame_bytes.data, + parsed_frame->can_dlc) == 0) + << "Parsed data bytes up to DLC should match expected data."; + + // 6. Upper layer application logic: Interpret the meaning of the payload + // bytes This is where you'd map CAN ID/DLC to specific data formats (e.g., + // protocol buffers, structs) Example: If ID 0x123 DLC 5 means a sensor value + // (uint16) at offset 1 + if (parsed_frame->can_id == 0x123 && + parsed_frame->can_dlc >= 1 + sizeof(uint16_t)) { + uint16_t sensor_value; + // Copy the bytes from the data array into the variable + std::memcpy(&sensor_value, parsed_frame->data + 1, + sizeof(uint16_t)); // Copy 2 bytes starting at index 1 + // In a real application/test, you would assert the value of sensor_value + // based on the bytes BB CC (assuming little-endian: 0xCCBB) + // ASSERT_EQ(sensor_value, 0xCCBB); + } else if (parsed_frame->can_id == 0x123 && + parsed_frame->can_dlc < 1 + sizeof(uint16_t)) { + // This branch is fine; indicates frame was received but doesn't have enough + // data for this specific parsing logic. + } +} + +TEST(CanStreamParsingTest, ParseStandardFrame_Multiple) { + // This test demonstrates parsing multiple standard CAN frames from a single + // buffer. + using namespace apollo::drivers::hal; + + const size_t frame_size = sizeof(struct can_frame); + + // Frame 1 + struct can_frame frame1_bytes; + frame1_bytes.can_id = 0x101; + frame1_bytes.can_dlc = 8; + std::memset(frame1_bytes.data, 0x11, 8); + // Frame 2 + struct can_frame frame2_bytes; + frame2_bytes.can_id = 0x102; + frame2_bytes.can_dlc = 4; + std::memset(frame2_bytes.data, 0x22, 4); + std::memset(frame2_bytes.data + 4, 0, 4); + // Frame 3 + struct can_frame frame3_bytes; + frame3_bytes.can_id = 0x103; + frame3_bytes.can_dlc = 0; + std::memset(frame3_bytes.data, 0x33, + 8); // Data contents irrelevant for DLC 0 + + // Prepare a buffer containing bytes for 3 frames + std::vector received_bytes(3 * frame_size); + std::memcpy(received_bytes.data(), &frame1_bytes, frame_size); + std::memcpy(received_bytes.data() + frame_size, &frame2_bytes, frame_size); + std::memcpy(received_bytes.data() + 2 * frame_size, &frame3_bytes, + frame_size); + + // --- Upper Layer Parsing Logic --- + uint8_t* buffer = received_bytes.data(); + size_t bytes_read = received_bytes.size(); + const size_t expected_frame_size = sizeof(struct can_frame); + + ASSERT_GE(bytes_read, expected_frame_size); + ASSERT_EQ(bytes_read % expected_frame_size, 0); + + size_t num_frames_received = bytes_read / expected_frame_size; + ASSERT_EQ(num_frames_received, 3); + + // Iterate through the buffer, parsing each frame + for (size_t i = 0; i < num_frames_received; ++i) { + // Calculate the start of the current frame in the buffer + const struct can_frame* parsed_frame = + reinterpret_cast(buffer + + i * expected_frame_size); + + // Verify each frame based on expected data + if (i == 0) { + ASSERT_EQ(parsed_frame->can_id, frame1_bytes.can_id); + ASSERT_EQ(parsed_frame->can_dlc, frame1_bytes.can_dlc); + ASSERT_TRUE(std::memcmp(parsed_frame->data, frame1_bytes.data, + parsed_frame->can_dlc) == 0); + } else if (i == 1) { + ASSERT_EQ(parsed_frame->can_id, frame2_bytes.can_id); + ASSERT_EQ(parsed_frame->can_dlc, frame2_bytes.can_dlc); + ASSERT_TRUE(std::memcmp(parsed_frame->data, frame2_bytes.data, + parsed_frame->can_dlc) == 0); + } else if (i == 2) { + ASSERT_EQ(parsed_frame->can_id, frame3_bytes.can_id); + ASSERT_EQ(parsed_frame->can_dlc, frame3_bytes.can_dlc); + ASSERT_TRUE(std::memcmp(parsed_frame->data, frame3_bytes.data, + parsed_frame->can_dlc) == 0); + } + // Application logic would then process parsed_frame->data based on ID/DLC + } +} + +TEST(CanStreamParsingTest, ParseCanFDFrame_Single_ValidLen) { + // This test demonstrates how upper layer parses bytes into a CAN FD frame. + using namespace apollo::drivers::hal; + + // 1. Manually create the byte representation of a CAN FD frame + struct canfd_frame raw_frame_fd_bytes; + raw_frame_fd_bytes.can_id = + 0x234 | CAN_EFF_FLAG | CANFD_BRS | CANFD_ESI; // Example ID + FD flags + raw_frame_fd_bytes.len = 32; // Example CAN FD data length (32 bytes) + raw_frame_fd_bytes.flags = 0; // Example FD flags (besides those in can_id) + + // Example data payload (32 bytes) + for (int i = 0; i < raw_frame_fd_bytes.len; ++i) { + raw_frame_fd_bytes.data[i] = static_cast<__u8>(i + 1); + } + std::memset(raw_frame_fd_bytes.data + raw_frame_fd_bytes.len, 0, + sizeof(raw_frame_fd_bytes.data) - + raw_frame_fd_bytes.len); // Zero pad remaining + + // Prepare the byte buffer as received by Stream::read + const size_t frame_size_fd = sizeof(struct canfd_frame); // e.g., 72 bytes + std::vector received_bytes_fd(frame_size_fd); + std::memcpy(received_bytes_fd.data(), &raw_frame_fd_bytes, frame_size_fd); + + // --- Upper Layer Parsing Logic --- + uint8_t* buffer_fd = received_bytes_fd.data(); + size_t bytes_read_fd = received_bytes_fd.size(); + + // 1. Upper layer must know the expected frame size (CAN FD in this case) + const size_t expected_frame_size_fd = sizeof(struct canfd_frame); + + // 2. Basic check: enough bytes for at least one frame, and total bytes is + // multiple of frame size + ASSERT_GE(bytes_read_fd, expected_frame_size_fd); + ASSERT_EQ(bytes_read_fd % expected_frame_size_fd, 0); + + // 3. Reinterpret the bytes as a CAN FD frame structure + const struct canfd_frame* parsed_frame_fd = + reinterpret_cast(buffer_fd); + + // 4. Access and verify frame fields (use len for data length) + ASSERT_EQ(parsed_frame_fd->can_id, raw_frame_fd_bytes.can_id); + ASSERT_EQ(parsed_frame_fd->len, raw_frame_fd_bytes.len); + ASSERT_EQ(parsed_frame_fd->flags, raw_frame_fd_bytes.flags); + + // 5. Access and verify data payload (only up to len) + ASSERT_EQ(parsed_frame_fd->len, 32); // Verify the expected data length + ASSERT_TRUE(std::memcmp(parsed_frame_fd->data, raw_frame_fd_bytes.data, + parsed_frame_fd->len) == 0) + << "Parsed data bytes up to len should match expected data."; + + // Upper layer application logic would then interpret parsed_frame_fd->data + // based on ID/len Example: Process the 32 bytes payload + if (parsed_frame_fd->len == 32) { + // Copy or process the 32 bytes in parsed_frame_fd->data + std::vector payload_data( + parsed_frame_fd->data, parsed_frame_fd->data + parsed_frame_fd->len); + ASSERT_EQ(payload_data.size(), 32); + for (int i = 0; i < 32; ++i) { + ASSERT_EQ(payload_data[i], static_cast(i + 1)); + } + } +} + +TEST(CanStreamParsingTest, ParseCanFDFrame_Multiple) { + // This test demonstrates parsing multiple CAN FD frames from a single buffer. + using namespace apollo::drivers::hal; + + const size_t frame_size_fd = sizeof(struct canfd_frame); + const size_t num_frames_to_test = 2; + + // Frame 1 (CAN FD) + struct canfd_frame frame_fd_1; + frame_fd_1.can_id = 0x301 | CAN_EFF_FLAG | CANFD_BRS; + frame_fd_1.len = 8; // CAN FD with standard DLC length + frame_fd_1.flags = 0; + std::memset(frame_fd_1.data, 0xA1, frame_fd_1.len); + std::memset(frame_fd_1.data + frame_fd_1.len, 0, + sizeof(frame_fd_1.data) - frame_fd_1.len); + + // Frame 2 (CAN FD) + struct canfd_frame frame_fd_2; + frame_fd_2.can_id = 0x302; // Standard ID + frame_fd_2.len = 64; // Max CAN FD length + frame_fd_2.flags = CANFD_ESI; + for (int i = 0; i < frame_fd_2.len; ++i) { + frame_fd_2.data[i] = static_cast<__u8>(63 - i); + } + + // Prepare a buffer containing bytes for 2 FD frames + std::vector received_bytes_fd(num_frames_to_test * frame_size_fd); + std::memcpy(received_bytes_fd.data(), &frame_fd_1, frame_size_fd); + std::memcpy(received_bytes_fd.data() + frame_size_fd, &frame_fd_2, + frame_size_fd); + + // --- Upper Layer Parsing Logic --- + uint8_t* buffer_fd = received_bytes_fd.data(); + size_t bytes_read_fd = received_bytes_fd.size(); + const size_t expected_frame_size_fd = sizeof(struct canfd_frame); + + ASSERT_GE(bytes_read_fd, expected_frame_size_fd); + ASSERT_EQ(bytes_read_fd % expected_frame_size_fd, 0); + + size_t num_frames_received_fd = bytes_read_fd / expected_frame_size_fd; + ASSERT_EQ(num_frames_received_fd, num_frames_to_test); + + // Iterate through the buffer, parsing each FD frame + for (size_t i = 0; i < num_frames_received_fd; ++i) { + const struct canfd_frame* parsed_frame_fd = + reinterpret_cast(buffer_fd + + i * expected_frame_size_fd); + + if (i == 0) { + ASSERT_EQ(parsed_frame_fd->can_id, frame_fd_1.can_id); + ASSERT_EQ(parsed_frame_fd->len, frame_fd_1.len); + ASSERT_EQ(parsed_frame_fd->flags, frame_fd_1.flags); + ASSERT_TRUE(std::memcmp(parsed_frame_fd->data, frame_fd_1.data, + parsed_frame_fd->len) == 0); + } else if (i == 1) { + ASSERT_EQ(parsed_frame_fd->can_id, frame_fd_2.can_id); + ASSERT_EQ(parsed_frame_fd->len, frame_fd_2.len); + ASSERT_EQ(parsed_frame_fd->flags, frame_fd_2.flags); + ASSERT_TRUE(std::memcmp(parsed_frame_fd->data, frame_fd_2.data, + parsed_frame_fd->len) == 0); + } + // Application logic would then process parsed_frame_fd->data based on + // ID/len/flags + } +} +// Add more parsing tests for different CAN FD lengths (0, 12, 16, 20, 24, 48). + +// --- Gtest Test Suite for CanStream Read Logic (Using Mock) --- +// These tests check CanStream::read's behavior using the mock. +// They focus on byte counts, return values, and exceptions. +// They DO NOT perform frame parsing. +TEST(CanStreamReadTest, ReadStandardFrame_Single_Success) { + // Test that CanStream::read successfully reads one standard frame via the + // mock. + using namespace apollo::drivers::hal; + using ::testing::_; // For matching any argument + + // Create mock stream for standard CAN (enable_can_fd = false) + MockCanStream mock_stream("vcan0", 0, 10000, false); + + // Define the byte representation of a standard frame to be "read" by the mock + const size_t frame_size = sizeof(struct can_frame); + std::vector frame_bytes(frame_size); + // Fill with some data (contents don't matter for this test, only size) + std::memset(frame_bytes.data(), 0x5A, frame_size); + + // Configure the mock to return this frame data when its read method is called + mock_stream.set_read_data(frame_bytes); + + // Set expectation that CanStream::read will call mock's read once + // Configure mock's read to use our action: Return the data set via + // set_read_data + EXPECT_CALL( + mock_stream, + read(_, + frame_size)) // CanStream::read asks for buffer of size frame_size + .WillOnce( + ::testing::Invoke(&mock_stream, &MockCanStream::MockReadAction)); + + // Call CanStream::read + std::vector read_buffer(frame_size); // Buffer to receive data + size_t bytes_read = mock_stream.read(read_buffer.data(), read_buffer.size()); + + // Assert the result: Should read exactly one frame size + ASSERT_EQ(bytes_read, frame_size); + + // Optional: Verify the bytes in the buffer match the original frame bytes + ASSERT_EQ(read_buffer, frame_bytes); +} + +TEST(CanStreamReadTest, ReadStandardFrame_Multiple_Success) { + // Test reading multiple standard frames in one call to CanStream::read + using namespace apollo::drivers::hal; + using ::testing::_; + + MockCanStream mock_stream("vcan0", 0, 10000, false); // standard CAN + + const size_t frame_size = sizeof(struct can_frame); + const size_t num_frames_to_read = 3; + + // Prepare byte data for multiple frames + std::vector mock_read_buffer(num_frames_to_read * frame_size); + // Fill with some dummy data + for (size_t i = 0; i < mock_read_buffer.size(); ++i) { + mock_read_buffer[i] = static_cast(i % 256); + } + + mock_stream.set_read_data(mock_read_buffer); + + // CanStream::read loops internally, but MockReadAction reads all available. + // So EXPECT_CALL on mock.read is likely only called once for the total size + // requested. + EXPECT_CALL(mock_stream, read(_, num_frames_to_read * frame_size)) + .WillOnce( + ::testing::Invoke(&mock_stream, &MockCanStream::MockReadAction)); + + // Call CanStream::read with buffer large enough for multiple frames + std::vector read_buffer(num_frames_to_read * frame_size); + size_t bytes_read = mock_stream.read(read_buffer.data(), read_buffer.size()); + + // Assert total bytes read + ASSERT_EQ(bytes_read, num_frames_to_read * frame_size); + // Optional: Verify the bytes in the buffer + ASSERT_EQ(read_buffer, mock_read_buffer); +} + +TEST(CanStreamReadTest, ReadStandardFrame_EAGAIN_ReturnsZero) { + // Test that CanStream::read returns 0 when the underlying read simulates + // EAGAIN (no data). + using namespace apollo::drivers::hal; + using ::testing::_; + using ::testing::Return; + using ::testing::SetErrno; + + MockCanStream mock_stream("vcan0", 0, 10000, false); // standard CAN + + const size_t frame_size = sizeof(struct can_frame); + + // Configure the mock read to return -1 and set errno to EAGAIN + EXPECT_CALL(mock_stream, read(_, frame_size)) + .WillOnce(SetErrno(EAGAIN)) + .WillOnce(Return(-1)); // Need 2 actions for SetErrno + Return(-1) + + // Call CanStream::read + std::vector read_buffer(frame_size); + size_t bytes_read = mock_stream.read(read_buffer.data(), read_buffer.size()); + + // Assert the result: Should return 0 bytes read + ASSERT_EQ(bytes_read, 0); + // You might check mock_stream.get_last_error_code() here too if exposed and + // set by CanStream::read +} + +TEST(CanStreamReadTest, ReadStandardFrame_FatalError_Throws) { + // Test that CanStream::read throws an exception on a fatal underlying read + // error. + using namespace apollo::drivers::hal; + using ::testing::_; + using ::testing::Return; + using ::testing::SetErrno; + + MockCanStream mock_stream("vcan0", 0, 10000, false); // standard CAN + + const size_size_t frame_size = sizeof(struct can_frame); + + // Configure the mock read to simulate a fatal error (e.g., EBADF) + EXPECT_CALL(mock_stream, read(_, frame_size)) + .WillOnce(SetErrno(EBADF)) + .WillOnce(Return(-1)); + + // Call CanStream::read and assert it throws std::runtime_error + std::vector read_buffer(frame_size); + ASSERT_THROW(mock_stream.read(read_buffer.data(), read_buffer.size()), + std::runtime_error); + // You might check mock_stream.get_last_error_code() after catching if it's + // set before throwing +} + +// --- CAN FD Read Tests --- + +TEST(CanStreamReadTest, ReadCanFDFrame_Single_Success) { + // Test that CanStream::read successfully reads one CAN FD frame via the mock. + using namespace apollo::drivers::hal; + using ::testing::_; + + // Create mock stream for CAN FD (enable_can_fd = true) + MockCanStream mock_stream("vcan0", 0, 10000, true); // true for CAN FD + + // Define the byte representation of a CAN FD frame + const size_t frame_size_fd = sizeof(struct canfd_frame); + std::vector frame_bytes_fd(frame_size_fd); + // Fill with some data (contents don't matter) + std::memset(frame_bytes_fd.data(), 0xFD, frame_size_fd); + + mock_stream.set_read_data(frame_bytes_fd); + + // Set expectation that CanStream::read will call mock's read once + EXPECT_CALL(mock_stream, + read(_, frame_size_fd)) // CanStream::read asks for buffer of + // size frame_size_fd + .WillOnce( + ::testing::Invoke(&mock_stream, &MockCanStream::MockReadAction)); + + // Call CanStream::read + std::vector read_buffer_fd(frame_size_fd); // Buffer to receive data + size_t bytes_read = + mock_stream.read(read_buffer_fd.data(), read_buffer_fd.size()); + + // Assert the result: Should read exactly one FD frame size + ASSERT_EQ(bytes_read, frame_size_fd); + // Optional: Verify the bytes + ASSERT_EQ(read_buffer_fd, frame_bytes_fd); +} + +TEST(CanStreamReadTest, ReadCanFDFrame_Multiple_Success) { + // Test reading multiple CAN FD frames in one call to CanStream::read + using namespace apollo::drivers::hal; + using ::testing::_; + + MockCanStream mock_stream("vcan0", 0, 10000, true); // CAN FD + + const size_t frame_size_fd = sizeof(struct canfd_frame); + const size_t num_frames_to_read = 2; + + // Prepare byte data for multiple FD frames + std::vector mock_read_buffer_fd(num_frames_to_read * frame_size_fd); + for (size_t i = 0; i < mock_read_buffer_fd.size(); ++i) { + mock_read_buffer_fd[i] = + static_cast(i % 100 + 100); // Different pattern + } + + mock_stream.set_read_data(mock_read_buffer_fd); + + EXPECT_CALL(mock_stream, read(_, num_frames_to_read * frame_size_fd)) + .WillOnce( + ::testing::Invoke(&mock_stream, &MockCanStream::MockReadAction)); + + std::vector read_buffer_fd(num_frames_to_read * frame_size_fd); + size_t bytes_read = + mock_stream.read(read_buffer_fd.data(), read_buffer_fd.size()); + + ASSERT_EQ(bytes_read, num_frames_to_read * frame_size_fd); + ASSERT_EQ(read_buffer_fd, mock_read_buffer_fd); +} + +TEST(CanStreamReadTest, ReadCanFDFrame_EAGAIN_ReturnsZero) { + // Test that CanStream::read returns 0 when underlying read simulates EAGAIN + // for FD. + using namespace apollo::drivers::hal; + using ::testing::_; + using ::testing::Return; + using ::testing::SetErrno; + + MockCanStream mock_stream("vcan0", 0, 10000, true); // CAN FD + const size_t frame_size_fd = sizeof(struct canfd_frame); + + EXPECT_CALL(mock_stream, read(_, frame_size_fd)) + .WillOnce(SetErrno(EAGAIN)) + .WillOnce(Return(-1)); + + std::vector read_buffer_fd(frame_size_fd); + size_t bytes_read = + mock_stream.read(read_buffer_fd.data(), read_buffer_fd.size()); + + ASSERT_EQ(bytes_read, 0); +} + +TEST(CanStreamReadTest, ReadCanFDFrame_FatalError_Throws) { + // Test that CanStream::read throws on fatal underlying read error for FD. + using namespace apollo::drivers::hal; + using ::testing::_; + using ::testing::Return; + using ::testing::SetErrno; + + MockCanStream mock_stream("vcan0", 0, 10000, true); // CAN FD + const size_t frame_size_fd = sizeof(struct canfd_frame); + + EXPECT_CALL(mock_stream, read(_, frame_size_fd)) + .WillOnce(SetErrno(EIO)) + .WillOnce(Return(-1)); // EIO as fatal example + + std::vector read_buffer_fd(frame_size_fd); + ASSERT_THROW(mock_stream.read(read_buffer_fd.data(), read_buffer_fd.size()), + std::runtime_error); +} + +// Add more read tests covering edge cases like connection lost (recv returns +// 0). + +// --- Gtest Test Suite for CanStream Write Logic (Using Mock) --- +// These tests check CanStream::write's behavior using the mock. +// They focus on byte counts, return values, and exceptions. +// They DO NOT perform frame construction or parsing. +TEST(CanStreamWriteTest, WriteStandardFrame_Single_Success) { + // Test successful writing of a single standard frame. + using namespace apollo::drivers::hal; + using ::testing::_; + + MockCanStream mock_stream("vcan0", 0, 10000, false); // standard CAN + + const size_t frame_size = sizeof(struct can_frame); + + // Prepare byte data for one frame (contents don't matter for this test) + std::vector data_to_write(frame_size); + std::memset(data_to_write.data(), 0xAA, frame_size); + + // Set expectation that CanStream::write will call mock's write once + // Configure mock's write to use our action: Accumulate written data and + // return size + EXPECT_CALL(mock_stream, write(::testing::An(), + frame_size)) // Expect call with correct size + .WillOnce( + ::testing::Invoke(&mock_stream, &MockCanStream::MockWriteAction)); + + // Call CanStream::write + size_t bytes_written = + mock_stream.write(data_to_write.data(), data_to_write.size()); + + // Assert the result: Should write exactly frame_size bytes + ASSERT_EQ(bytes_written, frame_size); + + // Verify the bytes were captured by the mock + ASSERT_EQ(mock_stream.get_written_data().size(), frame_size); + ASSERT_EQ(mock_stream.get_written_data(), data_to_write); +} + +TEST(CanStreamWriteTest, WriteStandardFrame_Multiple_Success) { + // Test successful writing of multiple standard frames in one call. + using namespace apollo::drivers::hal; + using ::testing::_; + + MockCanStream mock_stream("vcan0", 0, 10000, false); // standard CAN + + const size_t frame_size = sizeof(struct can_frame); + const size_t num_frames_to_write = 2; + + // Prepare byte data for multiple frames + std::vector data_to_write(num_frames_to_write * frame_size); + // Fill with dummy data + for (size_t i = 0; i < data_to_write.size(); ++i) { + data_to_write[i] = static_cast(i % 256); + } + + // CanStream::write loops internally, but MockWriteAction accumulates all + // writes. So EXPECT_CALL on mock.write is likely only called once for the + // total size requested. + EXPECT_CALL(mock_stream, + write(::testing::An(), data_to_write.size())) + .WillOnce( + ::testing::Invoke(&mock_stream, &MockCanStream::MockWriteAction)); + + // Call CanStream::write + size_t bytes_written = + mock_stream.write(data_to_write.data(), data_to_write.size()); + + // Assert the result + ASSERT_EQ(bytes_written, data_to_write.size()); + ASSERT_EQ(bytes_written, num_frames_to_write * frame_size); + + // Verify the bytes captured by the mock + ASSERT_EQ(mock_stream.get_written_data().size(), data_to_write.size()); + ASSERT_EQ(mock_stream.get_written_data(), data_to_write); +} + +TEST(CanStreamWriteTest, WriteStandardFrame_IncorrectLength_Throws) { + // Test that writing data whose length is not a multiple of frame size throws. + using namespace apollo::drivers::hal; + using ::testing::_; + + MockCanStream mock_stream("vcan0", 0, 10000, false); // standard CAN + + const size_t frame_size = sizeof(struct can_frame); + + // Data with length not a multiple of frame size + std::vector incorrect_data(frame_size + 5); // 5 bytes extra + + // Expect CanStream::write to throw std::invalid_argument because of length + // check The mock write method should NOT be called + EXPECT_CALL(mock_stream, write(_, _)).Times(0); + + // Call CanStream::write and assert it throws + ASSERT_THROW(mock_stream.write(incorrect_data.data(), incorrect_data.size()), + std::invalid_argument); +} + +TEST(CanStreamWriteTest, WriteStandardFrame_EAGAIN_ReturnsPartialOrZero) { + // Test that CanStream::write returns partial bytes or 0 when underlying send + // simulates EAGAIN. + using namespace apollo::drivers::hal; + using ::testing::_; + using ::testing::Invoke; + using ::testing::Return; + using ::testing::SetErrno; + + MockCanStream mock_stream("vcan0", 0, 10000, false); // standard CAN + const size_t frame_size = sizeof(struct can_frame); + const size_t num_frames = 3; // Attempt to write 3 frames + const size_t total_size = num_frames * frame_size; + + std::vector data_to_write(total_size); + for (size_t i = 0; i < data_to_write.size(); ++i) { + data_to_write[i] = static_cast(i); + } + + // Configure mock write action: + // CanStream::write loops internally, calling mock.write once per frame + // attempt. We expect calls for frame_size at a time. + EXPECT_CALL(mock_stream, write(::testing::An(), frame_size)) + .WillOnce(::testing::Invoke( + &mock_stream, &MockCanStream::MockWriteAction)) // Write frame 1 + .WillOnce(::testing::Invoke( + &mock_stream, &MockCanStream::MockWriteAction)) // Write frame 2 + .WillOnce(SetErrno(EAGAIN)) + .WillOnce(Return(-1)); // Attempt frame 3 (fails, sets errno, returns -1) + + // Call CanStream::write + size_t bytes_written = + mock_stream.write(data_to_write.data(), data_to_write.size()); + + // Assert the result: Should return bytes for 2 frames (2 * frame_size) + ASSERT_EQ(bytes_written, 2 * frame_size); + + // Verify that only the bytes for the first 2 frames were captured by the mock + ASSERT_EQ(mock_stream.get_written_data().size(), 2 * frame_size); + ASSERT_TRUE(std::memcmp(mock_stream.get_written_data().data(), + data_to_write.data(), 2 * frame_size) == 0); +} + +TEST(CanStreamWriteTest, WriteStandardFrame_FatalError_Throws) { + // Test that CanStream::write throws an exception on a fatal underlying write + // error. + using namespace apollo::drivers::hal; + using ::testing::_; + using ::testing::Invoke; + using ::testing::Return; + using ::testing::SetErrno; + + MockCanStream mock_stream("vcan0", 0, 10000, false); // standard CAN + const size_t frame_size = sizeof(struct can_frame); + const size_t num_frames = 2; // Attempt to write 2 frames + const size_t total_size = num_frames * frame_size; + + std::vector data_to_write(total_size); + std::memset(data_to_write.data(), 0xCC, total_size); + + // Configure mock write action: + // 1st frame: Succeeds + // 2nd frame: Fails with a fatal error (e.g., EIO) + EXPECT_CALL(mock_stream, write(::testing::An(), frame_size)) + .WillOnce(::testing::Invoke( + &mock_stream, &MockCanStream::MockWriteAction)) // Write frame 1 + .WillOnce(SetErrno(EIO)) + .WillOnce(Return(-1)); // Attempt frame 2 (fails fatally) + + // Call CanStream::write and assert it throws std::runtime_error + ASSERT_THROW(mock_stream.write(data_to_write.data(), data_to_write.size()), + std::runtime_error); + + // Verify that only the bytes for the first frame were captured by the mock + // before error + ASSERT_EQ(mock_stream.get_written_data().size(), frame_size); + ASSERT_TRUE(std::memcmp(mock_stream.get_written_data().data(), + data_to_write.data(), frame_size) == 0); + + // Check if Disconnect was called (assuming CanStream::write calls Disconnect + // on fatal error) EXPECT_CALL(mock_stream, + // Disconnect()).WillOnce(Return(true)); // Add expectation if testing this + // flow +} + +// --- CAN FD Write Tests --- + +TEST(CanStreamWriteTest, WriteCanFDFrame_Single_Success) { + // Test successful writing of a single CAN FD frame. + using namespace apollo::drivers::hal; + using ::testing::_; + + MockCanStream mock_stream("vcan0", 0, 10000, true); // CAN FD + const size_t frame_size_fd = sizeof(struct canfd_frame); + + std::vector data_to_write_fd(frame_size_fd); + std::memset(data_to_write_fd.data(), 0xFD, frame_size_fd); + + EXPECT_CALL(mock_stream, + write(::testing::An(), frame_size_fd)) + .WillOnce( + ::testing::Invoke(&mock_stream, &MockCanStream::MockWriteAction)); + + size_t bytes_written = + mock_stream.write(data_to_write_fd.data(), data_to_write_fd.size()); + + ASSERT_EQ(bytes_written, frame_size_fd); + ASSERT_EQ(mock_stream.get_written_data().size(), frame_size_fd); + ASSERT_EQ(mock_stream.get_written_data(), data_to_write_fd); +} + +TEST(CanStreamWriteTest, WriteCanFDFrame_Multiple_Success) { + // Test successful writing of multiple CAN FD frames. + using namespace apollo::drivers::hal; + using ::testing::_; + + MockCanStream mock_stream("vcan0", 0, 10000, true); // CAN FD + const size_t frame_size_fd = sizeof(struct canfd_frame); + const size_t num_frames = 2; + const size_t total_size = num_frames * frame_size_fd; + + std::vector data_to_write_fd(total_size); + for (size_t i = 0; i < data_to_write_fd.size(); ++i) { + data_to_write_fd[i] = + static_cast(i % 100 + 200); // Another pattern + } + + EXPECT_CALL(mock_stream, write(::testing::An(), total_size)) + .WillOnce( + ::testing::Invoke(&mock_stream, &MockCanStream::MockWriteAction)); + + size_t bytes_written = + mock_stream.write(data_to_write_fd.data(), data_to_write_fd.size()); + + ASSERT_EQ(bytes_written, total_size); + ASSERT_EQ(bytes_written, num_frames * frame_size_fd); + ASSERT_EQ(mock_stream.get_written_data().size(), total_size); + ASSERT_EQ(mock_stream.get_written_data(), data_to_write_fd); +} + +TEST(CanStreamWriteTest, WriteCanFDFrame_IncorrectLength_Throws) { + // Test that writing data whose length is not a multiple of FD frame size + // throws. + using namespace apollo::drivers::hal; + using ::testing::_; + + MockCanStream mock_stream("vcan0", 0, 10000, true); // CAN FD + const size_size_t frame_size_fd = sizeof(struct canfd_frame); + + std::vector incorrect_data_fd(frame_size_fd + 10); // 10 bytes extra + + EXPECT_CALL(mock_stream, write(_, _)).Times(0); + + ASSERT_THROW( + mock_stream.write(incorrect_data_fd.data(), incorrect_data_fd.size()), + std::invalid_argument); +} + +TEST(CanStreamWriteTest, WriteCanFDFrame_EAGAIN_ReturnsPartialOrZero) { + // Test that CanStream::write returns partial bytes or 0 when underlying send + // simulates EAGAIN for FD. + using namespace apollo::drivers::hal; + using ::testing::_; + using ::testing::Invoke; + using ::testing::Return; + using ::testing::SetErrno; + + MockCanStream mock_stream("vcan0", 0, 10000, true); // CAN FD + const size_t frame_size_fd = sizeof(struct canfd_frame); + const size_t num_frames = 2; + const size_t total_size = num_frames * frame_size_fd; + + std::vector data_to_write_fd(total_size); + std::memset(data_to_write_fd.data(), 0xDD, total_size); + + EXPECT_CALL(mock_stream, + write(::testing::An(), frame_size_fd)) + .WillOnce(::testing::Invoke( + &mock_stream, &MockCanStream::MockWriteAction)) // Write frame 1 + .WillOnce(SetErrno(EAGAIN)) + .WillOnce(Return(-1)); // Attempt frame 2 (fails) + + size_t bytes_written = + mock_stream.write(data_to_write_fd.data(), data_to_write_fd.size()); + + ASSERT_EQ(bytes_written, frame_size_fd); + ASSERT_EQ(mock_stream.get_written_data().size(), frame_size_fd); + ASSERT_TRUE(std::memcmp(mock_stream.get_written_data().data(), + data_to_write_fd.data(), frame_size_fd) == 0); +} + +TEST(CanStreamWriteTest, WriteCanFDFrame_FatalError_Throws) { + // Test that CanStream::write throws on fatal underlying write error for FD. + using namespace apollo::drivers::hal; + using ::testing::_; + using ::testing::Invoke; + using ::testing::Return; + using ::testing::SetErrno; + + MockCanStream mock_stream("vcan0", 0, 10000, true); // CAN FD + const size_t frame_size_fd = sizeof(struct canfd_frame); + const size_t num_frames = 2; + const size_t total_size = num_frames * frame_size_fd; + + std::vector data_to_write_fd(total_size); + std::memset(data_to_write_fd.data(), 0xEE, total_size); + + EXPECT_CALL(mock_stream, + write(::testing::An(), frame_size_fd)) + .WillOnce(::testing::Invoke( + &mock_stream, &MockCanStream::MockWriteAction)) // Write frame 1 + .WillOnce(SetErrno(EBADF)) + .WillOnce(Return(-1)); // Attempt frame 2 (fails fatally) + + ASSERT_THROW( + mock_stream.write(data_to_write_fd.data(), data_to_write_fd.size()), + std::runtime_error); + + ASSERT_EQ(mock_stream.get_written_data().size(), frame_size_fd); + ASSERT_TRUE(std::memcmp(mock_stream.get_written_data().data(), + data_to_write_fd.data(), frame_size_fd) == 0); +} diff --git a/modules/drivers/hal/stream/ntrip_stream.cc b/modules/drivers/hal/stream/ntrip_stream.cc new file mode 100644 index 00000000000..95341944dad --- /dev/null +++ b/modules/drivers/hal/stream/ntrip_stream.cc @@ -0,0 +1,561 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * 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 "modules/drivers/hal/stream/ntrip_stream.h" + +#include "cyber/cyber.h" + +namespace apollo { +namespace drivers { +namespace hal { + +// Constructor: Initializes parameters and creates the underlying TcpStream. +// Throws std::runtime_error if TcpStream construction fails. +NtripStream::NtripStream(const std::string& address, uint16_t port, + const std::string& mountpoint, const std::string& user, + const std::string& passwd, uint32_t timeout_s) + : mountpoint_(mountpoint), + address_(address), + port_(port), + write_data_prefix_("GET /" + mountpoint + + " HTTP/1.0\r\n" + "User-Agent: NTRIP gnss_driver/0.0\r\n" + "accept: */* \r\n\r\n"), + login_data_("GET /" + mountpoint + + " HTTP/1.0\r\n" + "User-Agent: NTRIP gnss_driver/0.0\r\n" + "accept: */* \r\n" + "Authorization: Basic " + + common::util::EncodeBase64(user + ":" + passwd) + "\r\n\r\n"), + timeout_s_(timeout_s) { + // Construct the initial GET request string (with or without authentication) + std::string auth_header; + if (!user.empty() || !passwd.empty()) { + auth_header = "Authorization: Basic " + + common::util::EncodeBase64(user + ":" + passwd) + "\r\n"; + } + + // Create the underlying TcpStream. Use timeout_usec = 0 for non-blocking + // reads/writes handled by NtripStream's select/pselect logic or its own + // timeouts. Let's pass a small timeout to TcpStream constructor, or use its + // default? Original used 0. Using 0 makes TcpStream non-blocking. NtripStream + // will manage the blocking/timeout using wait_readable/wait_writable if + // needed. Let's configure TcpStream with the *data activity timeout* for its + // read/write, or keep it non-blocking and handle waiting here. The most + // common is NtripStream handles timeouts. Let's pass 0 to TcpStream. + try { + // The TcpStream constructor should throw on invalid address/port format or + // other setup errors. + tcp_stream_ = std::make_unique(address_, port_, 0, false); + } catch (const std::exception& e) { + throw std::runtime_error("NtripStream failed to create TcpStream: " + + std::string(e.what())); + } + + AINFO << "NtripStream created for " << address_ << ":" << port_ << "/" + << mountpoint_; +} + +// Destructor: Disconnects the stream. +NtripStream::~NtripStream() { + AINFO << "NtripStream destructing for " << address_ << ":" << port_ << "/" + << mountpoint_ << ", disconnecting."; + this->Disconnect(); // Safe to call even if not connected +} + +// Connect: Establishes the TCP connection and performs the NTRIP handshake. +// Throws std::runtime_error for fatal errors during setup or handshake. +// Returns true on success. Returns false on failure (when not throwing). +bool NtripStream::Connect() { + std::lock_guard lock(internal_mutex_); + + if (is_login_) { + AINFO << "NtripStream already logged in."; + status_ = Stream::Status::CONNECTED; + return true; + } + + AINFO << "Attempting to connect and login to NTRIP caster " << address_ << ":" + << port_ << "/" << mountpoint_; + last_errno_ = 0; // Reset error code + + // Ensure tcp_stream_ is valid before use (should be valid due to constructor + // exception handling) + if (!tcp_stream_) { + last_errno_ = EBADF; // Use EBADF to indicate invalid internal state + AERROR << "NtripStream internal tcp_stream_ is invalid."; + status_ = Stream::Status::ERROR; + return false; // Cannot connect if internal state is bad + } + + try { + // 1. Connect the underlying TCP stream. + // TcpStream::Connect should throw on fatal setup/connect errors, return + // false on timeout/refused. + if (!tcp_stream_->Connect()) { + // tcp_stream_->Connect returned false (e.g., timeout, connection refused) + last_errno_ = tcp_stream_->get_last_error_code(); + AERROR << "NtripStream failed to connect TCP: " << strerror(last_errno_) + << " (errno: " << last_errno_ << ")."; + status_ = Stream::Status::DISCONNECTED; + // set this, but ensure + is_login_ = false; // Ensure login state is false + return false; // Connection failed (gracefully reported by TcpStream) + } + + // If TcpStream::Connect succeeded (returned true), proceed with NTRIP + // handshake. + + // 2. Send the NTRIP GET request (with or without authentication). + // TcpStream::write throws on fatal errors. + size_t sent_size = + tcp_stream_->write(reinterpret_cast(login_data_.data()), + login_data_.size()); + if (sent_size != login_data_.size()) { + // This case might indicate a partial write which TcpStream::write should + // handle, or it could indicate an error before throwing. The optimized + // TcpStream::write throws on fatal errors and returns partial size on + // EAGAIN/timeout. If it returns less than size *without* throwing, it + // implies EAGAIN/timeout. For handshake, we need the whole request sent. + last_errno_ = tcp_stream_->get_last_error_code(); // Get specific error + AERROR << "NtripStream failed to send full GET request during handshake. " + "Sent: " + << sent_size << ", expected: " << login_data_.size() + << ". Error: " << strerror(last_errno_); + tcp_stream_->Disconnect(); // Clean up connection + status_ = Stream::Status::ERROR; + is_login_ = false; + // Throw indicating handshake failure + throw std::runtime_error( + "NtripStream failed to send full handshake request."); + } + AINFO << "NtripStream sent GET request."; + + // 3. Read and parse the HTTP response header. + std::string response_header = read_http_header_locked(timeout_s_); + + AINFO << "NtripStream received response header (first few lines):"; + // Log first few lines of header + size_t end_of_first_line = response_header.find('\n'); + if (end_of_first_line != std::string::npos) { + AINFO << response_header.substr(0, end_of_first_line + 1); + size_t end_of_second_line = + response_header.find('\n', end_of_first_line + 1); + if (end_of_second_line != std::string::npos) { + AINFO << response_header.substr(end_of_first_line + 1, + end_of_second_line - end_of_first_line); + } + } else { + AINFO << response_header.substr( + 0, std::min(response_header.size(), + (size_t)100)); // Log up to 100 chars if no newline + } + + // 4. Parse the response header to check for success/failure. + bool handshake_success = false; + if (response_header.find("ICY 200 OK\r\n") != std::string::npos) { + // Successful NTRIP stream handshake + handshake_success = true; + AINFO << "Ntrip login successfully to mountpoint " << mountpoint_ << "."; + } else if (response_header.find("SOURCETABLE 200 OK\r\n") != + std::string::npos) { + // Received Source Table instead of stream (mountpoint probably wrong) + AERROR << "NtripStream received Source Table. Mountpoint '" << mountpoint_ + << "' may not exist or is incorrect."; + // This is a handshake failure, but not a network error. Log and fail. + } else if (response_header.find("HTTP/") != std::string::npos) { + // Received a standard HTTP error response (e.g., 401 Unauthorized, 404 + // Not Found) Check for specific codes if needed, but general HTTP + // response indicates failure. + AERROR << "NtripStream received HTTP error response during handshake."; + // Log the relevant part of the response if possible + size_t http_start = response_header.find("HTTP/"); + size_t end_of_status_line = response_header.find('\n', http_start); + if (http_start != std::string::npos && + end_of_status_line != std::string::npos) { + AERROR << "Status line: " + << response_header.substr(http_start, + end_of_status_line - http_start); + } + // This is a handshake failure. + } else { + // Unexpected response + AERROR << "NtripStream received unexpected response during handshake."; + // Log the response for debugging + AERROR << "Full unexpected response header:\n" << response_header; + } + + // 5. Finalize connection state based on handshake result. + if (handshake_success) { + is_login_ = true; + status_ = Stream::Status::CONNECTED; + data_active_s_ = + cyber::Time::Now().ToSecond(); // Initialize data activity timestamp + last_errno_ = 0; // Clear error + return true; // Connect successful + } else { + // Handshake failed based on response parsing. + tcp_stream_->Disconnect(); // Clean up TCP connection + status_ = Stream::Status::ERROR; + is_login_ = false; // Ensure login state is false + last_errno_ = + EPROTO; // Use EPROTO (Protocol error) or a custom error code + // Throw exception indicating handshake failure + throw std::runtime_error("NtripStream handshake failed."); + // return false; // Could return false instead of throwing for handshake + // failure if desired, but throwing is clearer. + } + + } catch (const std::exception& e) { + last_errno_ = tcp_stream_ ? tcp_stream_->get_last_error_code() : EBADF; + AERROR << "NtripStream Connect failed: " << e.what() + << " (Underlying errno: " << last_errno_ << ", " + << strerror(last_errno_) << ")."; + // Ensure disconnect and status update + if (tcp_stream_) { + tcp_stream_->Disconnect(); + } + is_login_ = false; + status_ = Stream::Status::ERROR; + + // Decide whether to rethrow the exception or just return false. + // Throwing is generally better for signaling *why* it failed setup/connect. + throw std::runtime_error("NtripStream Connect failed: " + + std::string(e.what())); + } +} + +// Disconnect: Disconnects the underlying TCP stream. +bool NtripStream::Disconnect() { + std::lock_guard lock( + internal_mutex_); // Lock during disconnection + + if (!is_login_) { + status_ = Stream::Status::DISCONNECTED; + AINFO << "NtripStream already disconnected."; + return true; + } + + AINFO << "Disconnecting NTRIP stream from " << address_ << ":" << port_ << "/" + << mountpoint_; + is_login_ = false; // Set login state to false immediately + + bool ret = true; + if (tcp_stream_) { + ret = tcp_stream_->Disconnect(); // Disconnect underlying TCP stream + if (!ret) { + last_errno_ = tcp_stream_->get_last_error_code(); + AERROR << "NtripStream failed to disconnect underlying TCP stream: " + << strerror(last_errno_); + } + } else { + // tcp_stream_ is null, considered already disconnected + AINFO << "NtripStream underlying tcp_stream_ is null, considered " + "disconnected."; + } + + status_ = Stream::Status::DISCONNECTED; + last_errno_ = 0; // Clear last error on successful disconnect + + return ret; +} + +// Attempts to disconnect and reconnect the NTRIP stream. +// Called internally on timeout or read/write errors. +// Assumes the caller holds the internal_mutex_ lock. +void NtripStream::Reconnect() { + // Mutex is already locked by the caller (read or write) + AINFO << "Reconnect: Attempting to reconnect ntrip caster " << address_ << ":" + << port_ << "/" << mountpoint_; + + // Disconnect the current stream state + // No need for lock guard here as we already hold the lock. + // Disconnect() method acquires lock internally, avoid double locking issues + // if possible. Or, make Disconnect_locked helper that assumes lock is held. + // Let's call the public Disconnect(). If it acquires lock internally, it's + // fine. Need to release lock before calling Disconnect() if Disconnect() + // acquires lock internally. A pattern could be: { + // std::unique_lock unlock(internal_mutex_); unlock.unlock(); + // Disconnect(); } lock.lock(); Or, make Disconnect_internal() that doesn't + // lock. Let's assume Disconnect() is safe to call even if lock is held (e.g., + // tries to acquire lock, or detects it's held). Or simplify: just call + // close() on the underlying tcp_stream_ + if (tcp_stream_) { + tcp_stream_->Disconnect(); // Call disconnect on underlying stream + } + is_login_ = false; // Ensure login state is false + status_ = Stream::Status::DISCONNECTED; + + // Attempt to connect again + try { + // Connect() acquires the lock internally, so need to release it here first. + // Releasing and re-acquiring the lock around the Connect call in + // Reconnect is crucial to avoid deadlock if Connect needs the lock + // internally, and to allow other threads to potentially acquire the lock if + // Reconnect blocks. Let's pass the lock to Connect, or ensure Connect + // doesn't block on the same lock. Simpler: Call Connect() which handles its + // own locking (if designed to be externally callable and thread-safe). If + // Connect() *must* be called with the mutex locked, then Reconnect + // needs Connect_locked helper. Let's assume Connect() can be called + // externally and handles its own required locking. So, release the lock + // *around* the Connect call. + std::unique_lock relock(internal_mutex_); + relock.unlock(); // Release lock before calling Connect + + bool connect_success = Connect(); + + relock.lock(); // Re-acquire lock after Connect returns or throws + + if (connect_success) { + data_active_s_ = cyber::Time::Now().ToSecond(); + AINFO << "Reconnect: Reconnected successfully."; + } else { + // Connect returned false + AERROR << "Reconnect: Connect attempt returned false."; + } + + } catch (const std::exception& e) { + // Catch exception from Connect attempt + last_errno_ = tcp_stream_ ? tcp_stream_->get_last_error_code() : EBADF; + AERROR << "Reconnect: Connect attempt threw: " << e.what() + << " (Underlying errno: " << last_errno_ << ", " + << strerror(last_errno_) << ")."; + } +} + +// Reads the HTTP response header from the TCP stream until "\r\n\r\n" or +// timeout. Assumes the caller holds the internal_mutex_ lock. Throws +// std::runtime_error on read error, timeout during header read, or if +// connection closes prematurely. Returns the received header string. +std::string NtripStream::read_http_header_locked(uint32_t timeout_s) { + if (!tcp_stream_ || !tcp_stream_->Connect()) { + last_errno_ = ENOTCONN; + throw std::runtime_error( + "read_http_header_locked called when tcp stream is not connected."); + } + + auto start_time = std::chrono::steady_clock::now(); + auto deadline = start_time + std::chrono::seconds(timeout_s); + + const std::string delimiter = "\r\n\r\n"; + const size_t max_header_size = 8192; + + std::string header_buffer; + header_buffer.reserve(2048); + + std::vector tmp_buf(512); + + constexpr uint32_t poll_interval_us = 10000; + + while (true) { + if (header_buffer.find(delimiter) != std::string::npos) { + break; + } + + auto now = std::chrono::steady_clock::now(); + if (now > deadline) { + last_errno_ = ETIMEDOUT; + throw std::runtime_error("read_http_header_locked timed out."); + } + + if (header_buffer.size() > max_header_size) { + last_errno_ = EMSGSIZE; + throw std::runtime_error( + "read_http_header_locked exceeded max header size."); + } + + bool can_read = false; + try { + can_read = tcp_stream_->Readable(poll_interval_us); + } catch (const std::exception& e) { + last_errno_ = tcp_stream_->get_last_error_code(); + throw std::runtime_error( + std::string("read_http_header_locked failed in Readable(): ") + + e.what()); + } + + if (!can_read) { + continue; + } + + ssize_t n = 0; + try { + n = tcp_stream_->read(tmp_buf.data(), tmp_buf.size()); + } catch (const std::exception& e) { + last_errno_ = tcp_stream_->get_last_error_code(); + throw std::runtime_error( + std::string("read_http_header_locked read() error: ") + e.what()); + } + + if (n > 0) { + header_buffer.append(reinterpret_cast(tmp_buf.data()), n); + last_errno_ = 0; + continue; + } + + if (n == 0) { + last_errno_ = ECONNRESET; + throw std::runtime_error( + "read_http_header_locked: connection closed by remote peer."); + } + + if (n < 0) { + int err = tcp_stream_->get_last_error_code(); + if (err == EAGAIN || err == EWOULDBLOCK) { + continue; + } + last_errno_ = err; + throw std::runtime_error( + std::string("read_http_header_locked fatal read error: ") + + std::strerror(err)); + } + } // end while + + return header_buffer; +} + +// Read data from the NTRIP stream. +size_t NtripStream::read(uint8_t* buffer, size_t max_length) { + std::lock_guard lock(internal_mutex_); // Lock for thread safety + + // Check connection status and potentially reconnect + if (!is_login_) { + // Not logged in. Attempt reconnect. Reconnect handles its own state + // and logging. + Reconnect(); + if (!is_login_) { + // Still not logged in after reconnect attempt. + last_errno_ = ENOTCONN; // Not connected + AERROR + << "NtripStream read failed: Not connected after reconnect attempt."; + return 0; // Cannot read + } + AINFO << "NtripStream read: Reconnected successfully."; + } + + // Check if tcp_stream_ is still valid after reconnect + if (!tcp_stream_) { + last_errno_ = EBADF; + AERROR << "NtripStream read failed: internal tcp_stream_ is null after " + "connect check."; + status_ = Stream::Status::ERROR; + return 0; // Cannot read + } + + size_t ret = 0; + last_errno_ = 0; // Reset errno before read attempt + + try { + // Delegate read to the underlying TcpStream. + // TcpStream::read returns >0 on data, 0 on timeout/would block, throws on + // fatal error/remote closed. The timeout_usec_ in TcpStream is 0 + // (non-blocking read kernel). This read call will return immediately with + // available data or 0 if none. + ret = tcp_stream_->read(buffer, max_length); + + if (ret > 0) { + // Successfully read data, update activity timestamp + data_active_s_ = cyber::Time::Now().ToSecond(); + last_errno_ = 0; // Clear error + } else if (ret == 0) { + // Read returned 0. This indicates timeout or non-blocking with no data + // available. TcpStream::read throws on remote closed. + last_errno_ = tcp_stream_->get_last_error_code(); + status_ = Stream::Status::CONNECTED; + } else { + // ret < 0 is not expected from optimized TcpStream::read (it throws). + // Defensive check, treat as error if it happens. + last_errno_ = tcp_stream_->get_last_error_code(); // Get underlying error + AERROR << "NtripStream read got unexpected negative return from " + "tcp_stream_->read: " + << ret << ". Underlying error: " << strerror(last_errno_); + // Throw to signal unexpected state. + Disconnect(); // Clean up + throw std::runtime_error( + "NtripStream read got unexpected return value from " + "tcp_stream_->read."); + } + + } catch (const std::exception& e) { + // Catch exceptions from TcpStream::read (fatal error, remote closed) + last_errno_ = tcp_stream_ ? tcp_stream_->get_last_error_code() : EBADF; + AERROR << "NtripStream read failed from tcp_stream_: " << e.what() + << " (Underlying errno: " << last_errno_ << ", " + << strerror(last_errno_) << ")."; + // This is a fatal error, need to disconnect and potentially reconnect. + Disconnect(); // Clean up + status_ = Stream::Status::ERROR; + is_login_ = false; // Ensure login state is false + + // Reconnect is handled by the check at the start of the read method on the + // *next* call. Throw exception to signal fatal error to the caller. + throw std::runtime_error("NtripStream read fatal error: " + + std::string(e.what())); + } + + // Check data activity timeout *after* handling the read result. + // Use cyber::Time::Now().ToSecond() directly for current time. + double current_time = cyber::Time::Now().ToSecond(); + if (current_time - data_active_s_ > timeout_s_) { + AINFO << "NtripStream data activity timeout (" << timeout_s_ + << " s). Last data: " << data_active_s_ + << ", Current time: " << current_time; + last_errno_ = ETIMEDOUT; // Set timeout error + // Reconnect if timeout occurs. Reconnect handles its own state. + Reconnect(); + // Reconnect updates is_login_ and status_. + // If reconnect fails, the next call to read will handle it. + // Return 0 bytes read for this timed-out call. + return 0; + } + + // Return the number of bytes read (0 on timeout/would_block, >0 on data). + return ret; +} + +// Write data to the NTRIP stream (intended for NMEA commands). +// Throws std::runtime_error for fatal write errors. +// Returns number of bytes written on success (equal to length if successful). +// Returns total bytes sent so far on timeout (EAGAIN/EWOULDBLOCK) or fatal +// error during partial write. Returns 0 if not connected. +size_t NtripStream::write(const uint8_t* buffer, size_t length) { + if (!tcp_stream_) { + return 0; + } + std::unique_lock lock(internal_mutex_, std::defer_lock); + if (!lock.try_lock()) { + AINFO << "Try lock failed."; + return 0; + } + + if (!is_login_ || tcp_stream_->get_status() != Stream::Status::CONNECTED) { + AERROR << "NtripStream write failed: Not connected or not logged in."; + return 0; + } + + size_t ret = tcp_stream_->write(buffer, length); + if (ret != length) { + AERROR << "Send ntrip data size " << length << ", return " << ret; + status_ = Stream::Status::ERROR; + return 0; + } + + return length; +} + +} // namespace hal +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/hal/stream/ntrip_stream.h b/modules/drivers/hal/stream/ntrip_stream.h new file mode 100644 index 00000000000..7c039796cef --- /dev/null +++ b/modules/drivers/hal/stream/ntrip_stream.h @@ -0,0 +1,119 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * 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. + *****************************************************************************/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "modules/common/util/string_util.h" +#include "modules/common/util/util.h" +#include "modules/drivers/hal/stream/stream.h" +#include "modules/drivers/hal/stream/tcp_stream.h" + +namespace apollo { +namespace drivers { +namespace hal { + +// Implements an NTRIP client stream built on top of a TcpStream. +// Handles NTRIP handshake (GET request, response parsing), data activity +// timeout, and auto-reconnection. +class NtripStream : public Stream { + public: + // Constructor takes NTRIP caster address, port, mountpoint, credentials, and + // a timeout. Timeout_s specifies the data activity timeout and handshake + // timeout. The underlying TcpStream will be configured with non-blocking + // reads/writes (timeout_usec = 0) unless a different behavior is desired for + // NTRIP data flow. Let's pass timeout_s converted to us for TcpStream? No, + // the original passed 0 to TcpStream. Let's keep TcpStream non-blocking and + // manage timeout in NtripStream. + NtripStream(const std::string& address, uint16_t port, + const std::string& mountpoint, const std::string& user, + const std::string& passwd, uint32_t timeout_s); + + // Destructor disconnects the stream and cleans up the TcpStream. + ~NtripStream() override; + + // Read data from the NTRIP stream (delegates to TcpStream). + // Handles data activity timeout and triggers reconnection. + // Thread-safe. + // Returns bytes read (> 0). + // Returns 0 on timeout or non-blocking with no data. + // Throws std::runtime_error on fatal errors or if device removed/connection + // lost. + size_t read(uint8_t* buffer, size_t max_length) override; + + // Write data to the NTRIP stream (delegates to TcpStream after handshake). + // Note: Standard NTRIP clients send NMEA/GGA commands after handshake, NOT + // GET headers. This method is intended for sending client commands like NMEA. + // Thread-safe. + // Returns bytes written (<= length). + // Returns 0 if not connected or on non-blocking EAGAIN/timeout. + // Throws std::runtime_error on fatal write errors. + size_t write(const uint8_t* data, size_t length) override; + + // Connect: Establishes the TCP connection and performs the NTRIP handshake. + // Returns true on success. Returns false on failure (including handshake + // failure). + bool Connect() override; + + // Disconnect: Disconnects the underlying TCP stream. + // Returns true on success. + bool Disconnect() override; + + int get_last_error_code() { + std::lock_guard lock(internal_mutex_); + return tcp_stream_ ? tcp_stream_->get_last_error_code() : 0; + } + + private: + // Attempts to disconnect and reconnect the NTRIP stream. + // Called internally on timeout or read/write errors. + // Assumes the caller holds the internal_mutex_ lock. + void Reconnect(); + + // Reads the HTTP response header from the TCP stream until "\r\n\r\n" or + // timeout. Throws std::runtime_error on read error or timeout during + // handshake. Returns the received header string. + std::string read_http_header_locked(uint32_t timeout_s); + + private: + bool is_login_ = false; // Indicates if the NTRIP handshake was successful + const std::string mountpoint_; + const std::string address_; + const uint16_t port_; + const std::string write_data_prefix_; + const std::string login_data_; + + uint32_t timeout_s_ = 60; // Data activity timeout in seconds + double data_active_s_ = 0.0; // Timestamp (seconds) of last received data + + std::unique_ptr tcp_stream_; // Underlying TCP stream + std::mutex internal_mutex_; +}; + +} // namespace hal +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/hal/stream/serial_stream.cc b/modules/drivers/hal/stream/serial_stream.cc new file mode 100644 index 00000000000..b8c145b0abc --- /dev/null +++ b/modules/drivers/hal/stream/serial_stream.cc @@ -0,0 +1,785 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * 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 "modules/drivers/hal/stream/serial_stream.h" + +#include "cyber/cyber.h" + +namespace apollo { +namespace drivers { +namespace hal { + +// Constructor: Stores configuration parameters. Doesn't open the port. +// Throws std::invalid_argument if device_name is empty. +SerialStream::SerialStream(const std::string& device_name, + baud_rate_t baud_rate, SerialByteSize bytesize, + SerialParity parity, SerialStopBits stopbits, + SerialFlowControl flowcontrol, uint32_t timeout_usec) + : device_name_(device_name), + baud_rate_(baud_rate), + bytesize_(bytesize), + parity_(parity), + stopbits_(stopbits), + flowcontrol_(flowcontrol), + timeout_usec_(timeout_usec), + fd_(-1), + is_open_(false) { + if (device_name_.empty()) { + // Use exception for invalid constructor arguments + throw std::invalid_argument("Serial device name cannot be empty."); + } +} + +SerialStream::~SerialStream() { + this->close(); // Safe to call multiple times +} + +// Helper function to open the serial port file descriptor. +// Throws std::runtime_error on failure. +void SerialStream::open(void) { + if (fd_ >= 0) { + // Already open + return; + } + + // Use O_RDWR for read/write access, O_NOCTTY to prevent it becoming the + // controlling terminal, O_NONBLOCK for non-blocking operation (termios + // VMIN/VTIME will control blocking behavior of read). Removed recursive call + // on EINTR. open should just fail on error. + int fd_ = ::open(device_name_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); + + if (fd_ == -1) { + last_errno_ = errno; + // Throw exception on failure to open device + throw std::runtime_error("Failed to open device " + device_name_ + + ", error: " + std::string(strerror(last_errno_))); + } + + // Configure the port settings using termios + try { + configure_port(); // configure_port throws on failure + } catch (const std::exception& e) { + // If configuration fails, close the file descriptor and re-throw + AERROR << "Failed to configure port " << device_name_ + << " after opening: " << e.what(); + ::close(fd_); + fd_ = -1; + // Re-throw the configuration error + throw; + } + + // If open and configure_port were successful + is_open_ = true; + last_errno_ = 0; // Clear last error on successful open + AINFO << "Successfully opened and configured serial port: " << device_name_; +} + +// Helper function to configure the serial port using termios. +// Assumes fd_ is valid and the port is open. +// Throws std::runtime_error on failure. +void SerialStream::configure_port() { + if (fd_ < 0) { + // Should not happen if called correctly after open(), but defensive check + throw std::runtime_error("configure_port called with invalid fd_"); + } + + struct termios options; // The options for the file descriptor + if (tcgetattr(fd_, &options) == -1) { + last_errno_ = errno; + throw std::runtime_error("tcgetattr failed for device " + device_name_ + + ": " + std::string(strerror(last_errno_))); + } + + // --- Apply Raw Mode / No Echo / Binary --- + // Enable receiver, ignore modem control lines + options.c_cflag |= (tcflag_t)(CLOCAL | CREAD); + // Disable canonical mode, echo, signals etc. + options.c_lflag &= + (tcflag_t) ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | ISIG | IEXTEN); + options.c_oflag &= (tcflag_t) ~(OPOST); // Disable output postprocessing + // Disable CR/NL handling, ignore break + options.c_iflag &= (tcflag_t) ~(INLCR | IGNCR | ICRNL | IGNBRK); + +#ifdef IUCLC // Not common, typically cleared + options.c_iflag &= (tcflag_t)~IUCLC; +#endif +#ifdef PARMRK // Typically cleared in raw mode + options.c_iflag &= (tcflag_t)~PARMRK; +#endif + + // --- Set Baud Rate --- + // Use standard POSIX functions for setting speeds. + if (cfsetispeed(&options, baud_rate_) == -1) { + last_errno_ = errno; + // Attempt to get numerical baud rate for logging if possible (requires + // mapping) + AERROR << "cfsetispeed failed for device " << device_name_ + << ", baud: " << baud_rate_ << ": " + << std::string(strerror(last_errno_)); + throw std::runtime_error("cfsetispeed failed."); + } + if (cfsetospeed(&options, baud_rate_) == -1) { + last_errno_ = errno; + AERROR << "cfsetospeed failed for device " << device_name_ + << ", baud: " << baud_rate_ << ": " + << std::string(strerror(last_errno_)); + throw std::runtime_error("cfsetospeed failed."); + } + + // --- Set Character Size (Data Bits) --- + options.c_cflag &= (tcflag_t)~CSIZE; // Clear current size bits + switch (bytesize_) { + case SerialByteSize::B5: + options.c_cflag |= CS5; + break; + case SerialByteSize::B6: + options.c_cflag |= CS6; + break; + case SerialByteSize::B7: + options.c_cflag |= CS7; + break; + case SerialByteSize::B8: + options.c_cflag |= CS8; + break; + default: + // This case should ideally not be reached if enum is used correctly, + // but handle defensively or validate in constructor. + throw std::invalid_argument("Unsupported byte size value."); + } + + // --- Set Stop Bits --- + switch (stopbits_) { + case SerialStopBits::One: + options.c_cflag &= (tcflag_t)~CSTOPB; + break; // 1 stop bit (clear CSTOPB) + case SerialStopBits::Two: + options.c_cflag |= CSTOPB; + break; // 2 stop bits (set CSTOPB) + default: + throw std::invalid_argument("Unsupported stop bits value."); + } + + // --- Set Parity --- + // Disable input parity checking and strip + options.c_iflag &= (tcflag_t) ~(INPCK | ISTRIP); + // Disable parity enable and odd parity + options.c_cflag &= (tcflag_t) ~(PARENB | PARODD); + switch (parity_) { + case SerialParity::None: + // PARENB is already cleared above + break; + case SerialParity::Even: + options.c_cflag |= PARENB; // Enable parity + // PARODD is already cleared above for Even + break; + case SerialParity::Odd: + options.c_cflag |= PARENB | PARODD; // Enable parity and set Odd + break; + default: + throw std::invalid_argument("Unsupported parity value."); + } + +// --- Set Flow Control --- +// Clear all flow control flags first +#ifdef IXANY + // Clear XON/XOFF flags + options.c_iflag &= (tcflag_t) ~(IXON | IXOFF | IXANY); +#else + options.c_iflag &= (tcflag_t) ~(IXON | IXOFF); +#endif +#ifdef CRTSCTS // Clear hardware flow control flags (RTS/CTS) + options.c_cflag &= static_cast(~(CRTSCTS)); +#elif defined CNEW_RTSCTS + options.c_cflag &= static_cast(~(CNEW_RTSCTS)); +#else + AINFO << "Neither CRTSCTS nor CNEW_RTSCTS defined. Hardware flow control " + "might not be supported."; +#endif + + switch (flowcontrol_) { + case SerialFlowControl::None: + // All flow control flags are cleared above + break; + case SerialFlowControl::XONXOFF: + options.c_iflag |= (tcflag_t)(IXON | IXOFF); // Enable XON/XOFF +#ifdef IXANY // If available, allow any character to restart output + options.c_iflag |= (tcflag_t)IXANY; +#endif + break; + case SerialFlowControl::RTSCTS: +#ifdef CRTSCTS // Standard POSIX hardware flow control + options.c_cflag |= static_cast(CRTSCTS); +#elif defined CNEW_RTSCTS // Alternative non-standard macro + options.c_cflag |= static_cast(CNEW_RTSCTS); +#else + // Hardware flow control requested but not supported by OS macros + throw std::runtime_error( + "RTS/CTS flow control requested but not supported by OS."); +#endif + break; + default: + throw std::invalid_argument("Unsupported flow control value."); + } + + // --- Set Read Timeout and VMIN/VTIME --- + // http://www.unixwiz.net/techtips/termios-vmin-vtime.html + // VMIN=0, VTIME=0: Non-blocking read in the kernel. read returns immediately + // with available data, or 0 if no data. This is used with select/poll/epoll. + // The timeout_usec_ parameter in this class controls the wait duration + // *before* calling read(), implemented using wait_readable/wait_writable. + options.c_cc[VMIN] = 0; + options.c_cc[VTIME] = 0; + + // --- Activate Settings --- + // TCSANOW: apply changes immediately + // TCSAFLUSH: flush input and output queues and apply changes (good practice + // after configuring) + if (tcsetattr(fd_, TCSAFLUSH, &options) == -1) { + last_errno_ = errno; + throw std::runtime_error("tcsetattr failed for device " + device_name_ + + ": " + std::string(strerror(last_errno_))); + } + + // --- Calculate Byte Time --- + // Calculate approximate time to transmit one byte. Used potentially for write + // timeouts. Need a reliable way to get numerical baud rate from speed_t + // constant. Assuming a helper get_numerical_baudrate(speed_t constant) -> + // uint32_t exists. For demonstration, using a simplified approximate + // calculation based on a potentially available numerical value. + // DANGEROUS - speed_t is not always numerical baud rate + uint32_t numerical_baud = static_cast(baud_rate_); + // **Real solution needs a reliable speed_t to numerical baud rate mapping.** + // Example: uint32_t numerical_baud = get_numerical_baudrate(baud_rate_); + + if (numerical_baud > 0) { + // Total bits per char approx = 1 start + data bits + parity + stop bits + // Common approximation is 10 bits/byte. + uint32_t total_bits_per_char = 10; + // microseconds per char + byte_time_us_ = (total_bits_per_char * 1000000) / numerical_baud; + AINFO << "Calculated byte time for device " << device_name_ << " at baud " + << numerical_baud << " is approx " << byte_time_us_ << " us."; + } else { + byte_time_us_ = 0; + AERROR << "Cannot calculate byte time for device " << device_name_ + << " due to unknown or zero baud rate value."; + } +} + +// Helper function to close the serial port. Safe to call multiple times. +void SerialStream::close(void) { + if (fd_ >= 0) { // Check if port is open + AINFO << "Closing serial port " << device_name_ << " fd: " << fd_; + // Flush any pending output before closing + tcflush(fd_, TCOFLUSH); + + if (::close(fd_) < 0) { + last_errno_ = errno; + AERROR << "Failed to close serial port " << device_name_ << " fd " << fd_ + << ": " << strerror(last_errno_); + // Don't throw on close failure in destructor or Disconnect. + } + fd_ = -1; + is_open_ = false; + } + status_ = Stream::Status::DISCONNECTED; +} + +// Connect: Opens and configures the serial port. +// Returns true if successful or already connected. +// Returns false on connection failure (when not throwing). +// Throws std::runtime_error for fatal setup errors during open/configure. +bool SerialStream::Connect() { + if (is_open_) { + status_ = Stream::Status::CONNECTED; // Ensure status is correct + return true; + } + + last_errno_ = 0; // Reset error code for this attempt + + try { + // open() handles opening the FD and calls configure_port(). + // Both open() and configure_port() throw on failure. + this->open(); + status_ = Stream::Status::CONNECTED; + AINFO << "Serial port " << device_name_ << " connected successfully."; + last_errno_ = 0; // Clear error on success + return true; + + } catch (const std::exception& e) { + // Catch exceptions thrown by open() or configure_port() + AERROR << "Failed to Connect to serial port " << device_name_ << ": " + << e.what(); + // fd_ might already be -1 if configure_port failed after open, but ensure + // it. + if (fd_ >= 0) { + ::close(fd_); + fd_ = -1; + } + is_open_ = false; + status_ = Stream::Status::ERROR; + return false; // Connection failed + } +} + +// Disconnect: Closes the serial port. +bool SerialStream::Disconnect() { + if (!is_open_) { + status_ = Stream::Status::DISCONNECTED; + return false; + } + + AINFO << "Disconnecting serial port " << device_name_ << " fd: " << fd_; + this->close(); // close() sets fd_ = -1, is_open_=false, and updates status_ + status_ = Stream::Status::DISCONNECTED; + last_errno_ = 0; // Clear last error on successful disconnect + return true; +} + +// Heuristic check to detect if the device has been removed. +// Attempts a zero-byte write. If certain errors occur (EBADF, EIO), +// logs, disconnects, and attempts to reconnect. +// Returns true if device likely removed and disconnected. +// Returns false otherwise. +bool SerialStream::check_remove() { + if (!is_open_ || fd_ < 0) return false; // Cannot check if not open + + char data = 0; + // Attempt a zero-byte write to provoke an EBADF or EIO if the device is gone. + // Using ::write with 0 length is a common trick. + ssize_t nsent = ::write(fd_, &data, 0); + + if (nsent < 0) { + // Error occurred. Check if it indicates device removal. + switch (errno) { + case EBADF: // Bad file descriptor + case EIO: // I/O error (common when device is removed) + case ENXIO: // No such device or address (less common on write after + // open) + // case EACCES: // Permissions error (less common after successful open) + last_errno_ = errno; + AERROR << "Serial stream detect write failed for device " + << device_name_ << ", error: " << strerror(last_errno_) + << ". Device likely removed."; + // Device is likely removed, disconnect and attempt reconnect. + Disconnect(); + // Note: Reconnect is attempted by the caller (read/write loop) if + // enabled. + return true; // Indicate device was likely removed + case EAGAIN: // Same as EWOULDBLOCK + case EINTR: // Interrupted system call - shouldn't happen for 0 bytes + // These are not device removal errors for a 0-byte write. + // Log a warning if seen, but don't treat as removed. + AINFO << "Serial check_remove got unexpected error " << strerror(errno) + << " for 0-byte write."; + return false; + default: + last_errno_ = errno; + AERROR << "Serial stream detect write failed with unhandled error for " + "device " + << device_name_ << ", error: " << strerror(last_errno_) + << " (errno: " << last_errno_ << ")."; + // Could be a temporary error or a different fatal error. + // Decide if this state should also trigger a Disconnect/Reconnect. + // For now, just return false and let read/write handle other errors. + return false; + } + } + return false; +} + +// Read data from the serial port. +// Throws std::runtime_error for fatal read errors or remote closed/device +// removed. Returns number of bytes read on success (> 0). Returns 0 on timeout +// or non-blocking with no data (EAGAIN/EWOULDBLOCK). Returns 0 if device not +// open and auto_reconnect fails or is disabled. +size_t SerialStream::read(uint8_t* buffer, size_t max_length) { + // Attempt auto-connect/reconnect if needed and enabled (Stream base likely + // handles this) Original code pattern: if (!is_open_) { if (!Connect()) + // return 0; } Let's keep this pattern, assuming Stream base doesn't handle + // auto-connect on read/write. + if (!is_open_) { + last_errno_ = ENOTCONN; // Set error before attempting connect + if (!Connect()) { + AERROR << "Read called when not connected and failed to connect."; + return 0; + } + AINFO << "Read called on disconnected port, connected successfully."; + } + + if (buffer == nullptr || max_length == 0) { + return 0; // Nothing to read into + } + + ssize_t bytes_current_read = 0; + + // Use wait_readable to handle timeout or non-blocking wait based on + // timeout_usec_ wait_readable returns false on timeout or select error. It + // sets last_errno_ on select error (r < 0). + if (!wait_readable(timeout_usec_)) { + // wait_readable returned false. Check if it was an error or a timeout. + if (last_errno_ != 0 && + last_errno_ != ETIMEDOUT) { // Check if wait failed with a real error + // (not just timeout) + AERROR << "Serial read wait_readable failed with select error: " + << strerror(last_errno_) << " (errno: " << last_errno_ + << "), fd: " << fd_; + status_ = Stream::Status::ERROR; + Disconnect(); // Clean up + throw std::runtime_error("Serial read wait failed: " + + std::string(strerror(last_errno_))); + } + check_remove(); + return 0; + } + + // Data is ready (wait_readable returned true) + // Read available data. With VMIN=0, VTIME=0, read will return immediately + // with however many bytes are in the buffer (up to max_length), or 0 if + // buffer emptied *after* wait_readable indicated data was coming (race + // condition possible but rare). Use a loop to handle EINTR in read itself, + // although less likely after pselect. + do { + bytes_current_read = ::read(fd_, buffer, max_length); + } while (bytes_current_read < 0 && errno == EINTR); + + if (bytes_current_read < 0) { + // Error during the actual read() call + switch (errno) { + case EAGAIN: // Same as EWOULDBLOCK + // Treat as no data read in this instant, although unexpected. + last_errno_ = errno; // Store for debugging + AINFO << "Serial read returned EAGAIN/EWOULDBLOCK after wait_readable. " + "FD: " + << fd_ << ", Error: " << strerror(last_errno_); + return 0; // Return 0 bytes read + case EINVAL: // Invalid argument (shouldn't happen with valid + // fd/buffer/length) + last_errno_ = errno; + AERROR << "Serial read returned EINVAL. FD: " << fd_ + << ", Error: " << strerror(last_errno_); + // Fatal error + Disconnect(); // Clean up + throw std::runtime_error("Serial read fatal error (EINVAL): " + + std::string(strerror(last_errno_))); + case EBADF: // Bad file descriptor - socket likely closed underneath + case EIO: // I/O error - device likely removed + last_errno_ = errno; + AERROR << "Serial stream read data failed, likely disconnected or " + "removed. Error: " + << strerror(last_errno_) << " (errno: " << last_errno_ + << "), fd: " << fd_; + status_ = Stream::Status::ERROR; + Disconnect(); // Clean up the broken fd + // Throw exception to signal fatal error + throw std::runtime_error("Serial read fatal error: " + + std::string(strerror(last_errno_))); + default: + last_errno_ = errno; + AERROR << "Serial stream read data failed with unhandled error: " + << strerror(last_errno_) << " (errno: " << last_errno_ + << "), fd: " << fd_; + status_ = Stream::Status::ERROR; + Disconnect(); // Clean up in case it's fatal + throw std::runtime_error("Serial read unhandled fatal error: " + + std::string(strerror(last_errno_))); + } + } + + if (bytes_current_read == 0) { + // read returned 0 bytes. With VMIN=0, VTIME=0 after wait_readable, + // this means no data was in the buffer at that exact moment, OR port + // closed. Treat as no data read in this call. check_remove provides a + // heuristic for closed. + last_errno_ = 0; // Clear errno for successful 0-byte read + status_ = Stream::Status::CONNECTED; + AINFO << "Serial read returned 0 bytes after wait_readable indicated data. " + "FD: " + << fd_ << ". Checking device status."; + // Call check_remove() to see if it indicates disconnection + if (check_remove()) { + // check_remove already logged, disconnected, and potentially attempted + // reconnect. If check_remove returned true, the device is gone. Throw + // exception. + throw std::runtime_error("Serial read detected device removed."); + } + return 0; + } + + // Success: bytes_current_read > 0 + // Return the actual number of bytes read in this call. + return bytes_current_read; +} + +// Write data to the serial port. +// Throws std::runtime_error for fatal write errors. +// Returns number of bytes written on success (equal to length if successful). +// Returns total bytes sent so far on timeout (EAGAIN/EWOULDBLOCK) or fatal +// error during partial write. Returns 0 if device not open and auto_reconnect +// fails or is disabled. +size_t SerialStream::write(const uint8_t* data, size_t length) { + // Attempt auto-connect/reconnect if needed and enabled + if (!is_open_) { + last_errno_ = ENOTCONN; // Set error before attempting connect + if (!Connect()) { + AERROR << "Write called when not connected and failed to connect."; + return 0; + } + AINFO << "Write called on disconnected port, connected successfully."; + } + + if (data == nullptr || length == 0) { + return 0; // Nothing to write + } + + size_t total_nsent = 0; + const uint8_t* current_data = data; + size_t remaining_length = length; + + // Write loop for partial writes (possible depending on kernel buffer) + while (remaining_length > 0) { + ssize_t nsent; + // Use a loop for EINTR + do { + nsent = ::write(fd_, current_data, remaining_length); + } while (nsent < 0 && errno == EINTR); + + if (nsent < 0) { + // Handle errors other than EINTR + if (errno == EAGAIN) { + // Non-blocking socket cannot send immediately (buffer full) + // Need to wait for buffer space using wait_writable. + last_errno_ = errno; // Store EAGAIN/EWOULDBLOCK + AINFO << "Serial write would block (EAGAIN/EWOULDBLOCK) after sending " + << total_nsent << " bytes. Waiting for buffer. FD: " << fd_; + // Use wait_writable with timeout_usec_ + if (wait_writable(timeout_usec_)) { + // Buffer is writable, continue the write loop to retry send + last_errno_ = 0; // Clear error if wait succeeded + continue; + } else { + // wait_writable timed out or had an error. + // last_errno_ is set by wait_writable on error/timeout. + AERROR << "Serial write wait_writable failed after sending " + << total_nsent << " bytes. Error: " << strerror(last_errno_) + << " (errno: " << last_errno_ << "), fd: " << fd_; + // Decide if wait_writable failure on EAGAIN should be fatal. + // Returning total_nsent indicates partial write and couldn't write + // more within timeout. Check if wait_writable failed with a real + // select error or just timeout. + if (last_errno_ != EAGAIN && last_errno_ != ETIMEDOUT && + last_errno_ != 0) { + // This is an error *during* the wait (not timeout), or a non-AGAIN + // error from send. + status_ = Stream::Status::ERROR; + Disconnect(); // Clean up + throw std::runtime_error("Serial write wait_writable failed: " + + std::string(strerror(last_errno_))); + } + // If we are here, it was EAGAIN on write, and wait_writable timed out + // or got EAGAIN/EWOULDBLOCK. Return partial write count. Caller + // should handle retry. + return total_nsent; + } + } else { + // Other serious errors (e.g., EIO, EBADF, ENXIO etc.) + last_errno_ = errno; + AERROR << "Serial stream write fatal error: " << strerror(last_errno_) + << " (errno: " << last_errno_ << "), fd: " << fd_; + status_ = Stream::Status::ERROR; + Disconnect(); // Clean up + // Throw exception after sending partial data. + throw std::runtime_error("Serial write fatal error: " + + std::string(strerror(last_errno_))); + } + } + + // nsent >= 0 : Bytes were sent (could be 0 if length was 0 or buffer full + // with non-blocking, but handled by EAGAIN above) + total_nsent += nsent; + current_data += nsent; + remaining_length -= nsent; + + // If nsent == 0 and not EAGAIN, this is strange. Could be an error. + // Treat similar to EAGAIN - wait for writable and retry. + if (nsent == 0 && remaining_length > 0) { + AINFO << "Serial write returned 0 bytes (not EAGAIN) after sending " + << total_nsent << " bytes. Waiting for buffer. FD: " << fd_; + last_errno_ = 0; // Clear errno as 0 return isn't necessarily error + if (wait_writable(timeout_usec_)) { + continue; // Retry write loop + } else { + // wait_writable failed or timed out + AERROR << "Serial write wait_writable failed after 0-byte write: " + << strerror(last_errno_) << " (errno: " << last_errno_ + << "), fd: " << fd_; + if (last_errno_ != EAGAIN && last_errno_ != ETIMEDOUT && + last_errno_ != 0) { // Real select error? + Disconnect(); // Clean up + throw std::runtime_error( + "Serial write wait_writable failed after 0-byte write: " + + std::string(strerror(last_errno_))); + } + // Timeout or EAGAIN from wait. Return partial write count. + return total_nsent; + } + } + } + + // If loop completes, all bytes were sent. + // last_errno_ = 0; // Clear error on success + return total_nsent; // Should be equal to original 'length' +} + +// Waits for the file descriptor to become readable using pselect. +// Returns true if readable, false on timeout or error. +// Sets last_errno_ on select error (r < 0) or timeout (ETIMEDOUT). +bool SerialStream::wait_readable(uint32_t timeout_us) { + if (fd_ < 0) { + last_errno_ = EBADF; // Indicate bad file descriptor + return false; // Cannot wait on invalid fd + } + + timespec timeout_ts; + fd_set readfds; + FD_ZERO(&readfds); + FD_SET(fd_, &readfds); + + // Correct pselect call: pass &timeout_ts only if timeout_us > 0 for a finite + // timeout. If timeout_us == 0, {0,0} timespec is used for immediate check + // (non-blocking wait). + struct timespec immediate_timeout = {0, 0}; // For timeout_us == 0 case + const struct timespec* pselect_timeout_ptr = + (timeout_us == 0) ? &immediate_timeout : &timeout_ts; + + if (timeout_us > 0) { + timeout_ts.tv_sec = timeout_us / 1000000; + timeout_ts.tv_nsec = (timeout_us % 1000000) * 1000; + } + + // Use pselect for signal-safe wait + int r = pselect(fd_ + 1, &readfds, NULL, NULL, pselect_timeout_ptr, NULL); + + if (r < 0) { + // Error during select/pselect call (e.g., bad file descriptor, signal) + if (errno == EINTR) { + // Interrupted by signal. Return false. Caller can decide to loop or + // handle. + last_errno_ = errno; + AINFO << "Serial read wait_readable interrupted by signal (EINTR)."; + return false; // Interrupted + } else { + last_errno_ = errno; + AERROR << "Serial read wait_readable failed with select error: " + << strerror(last_errno_) << " (errno: " << last_errno_ + << "), fd: " << fd_; + status_ = Stream::Status::ERROR; + // Fatal select error. Return false. Caller should check last_errno_. + return false; // Error + } + } else if (r == 0) { + // Timeout occurred + last_errno_ = ETIMEDOUT; // Use ETIMEDOUT to indicate timeout + status_ = Stream::Status::CONNECTED; + return false; // Timeout + } else { // r > 0 + // File descriptor is ready + // This shouldn't happen if r > 0 our fd has to be in the list, but + // defensive check + if (FD_ISSET(fd_, &readfds)) { + last_errno_ = 0; // Clear error on success + return true; // Readable + } else { + // Should not be reachable if r > 0 for a single FD set. + // Indicates an unexpected state or issue with the file descriptor. + last_errno_ = EBADF; // Indicate unexpected state / bad fd + AERROR << "Serial read wait_readable: pselect returned > 0 but FD_ISSET " + "is false. Unexpected. FD: " + << fd_; + status_ = Stream::Status::ERROR; + return false; // Unexpected state / Error + } + } +} + +// Waits for the file descriptor to become writable using pselect. +// Returns true if writable, false on timeout or error. +// Sets last_errno_ on select error (r < 0) or timeout (ETIMEDOUT). +bool SerialStream::wait_writable(uint32_t timeout_us) { + if (fd_ < 0) { + last_errno_ = EBADF; // Indicate bad file descriptor + return false; // Cannot wait on invalid fd + } + + timespec timeout_ts; + fd_set writefds; + FD_ZERO(&writefds); + FD_SET(fd_, &writefds); + + // Correct pselect call: pass &timeout_ts only if timeout_us > 0 for a finite + // timeout. If timeout_us == 0, {0,0} timespec is used for immediate check + // (non-blocking wait). + struct timespec immediate_timeout = {0, 0}; // For timeout_us == 0 case + const struct timespec* pselect_timeout_ptr = + (timeout_us == 0) ? &immediate_timeout : &timeout_ts; + + if (timeout_us > 0) { + timeout_ts.tv_sec = timeout_us / 1000000; + timeout_ts.tv_nsec = (timeout_us % 1000000) * 1000; + } + + // Use pselect for signal-safe wait + // Only ONE pselect call. + int r = pselect(fd_ + 1, NULL, &writefds, NULL, pselect_timeout_ptr, NULL); + + if (r < 0) { + // Error during select/pselect call + if (errno == EINTR) { + last_errno_ = errno; + AINFO << "Serial write wait_writable interrupted by signal (EINTR)."; + return false; // Interrupted + } else { + last_errno_ = errno; + AERROR << "Serial write wait_writable failed with select error: " + << strerror(last_errno_) << " (errno: " << last_errno_ + << "), fd: " << fd_; + status_ = Stream::Status::ERROR; + return false; // Error + } + } else if (r == 0) { + // Timeout occurred + last_errno_ = ETIMEDOUT; // Use ETIMEDOUT + status_ = Stream::Status::CONNECTED; + return false; // Timeout + } else { // r > 0 + // File descriptor is ready + if (FD_ISSET(fd_, &writefds)) { + last_errno_ = 0; // Clear error + return true; // Writable + } else { + last_errno_ = EBADF; // Indicate unexpected state / bad fd + AERROR << "Serial write wait_writable: pselect returned > 0 but FD_ISSET " + "is false. Unexpected. FD: " + << fd_; + status_ = Stream::Status::ERROR; + return false; // Unexpected state / Error + } + } +} + +} // namespace hal +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/hal/stream/serial_stream.h b/modules/drivers/hal/stream/serial_stream.h new file mode 100644 index 00000000000..cb420877785 --- /dev/null +++ b/modules/drivers/hal/stream/serial_stream.h @@ -0,0 +1,147 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * 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. + *****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "modules/drivers/hal/stream/stream.h" + +namespace apollo { +namespace drivers { +namespace hal { + +// Enum for common serial port configurations +enum class SerialByteSize { B5, B6, B7, B8 }; +enum class SerialParity { None, Even, Odd }; +enum class SerialStopBits { One, Two }; +enum class SerialFlowControl { None, XONXOFF, RTSCTS }; + +class SerialStream : public Stream { + using baud_rate_t = speed_t; + + public: + // Constructor takes device name, baud rate constant, timeout, and other port + // settings. timeout_usec = 0 for non-blocking reads/writes (will return + // immediately if no data/buffer). timeout_usec > 0 implements a timeout for + // read/write waiting using pselect. + SerialStream(const std::string& device_name, baud_rate_t baud_rate, + SerialByteSize bytesize, SerialParity parity, + SerialStopBits stopbits, SerialFlowControl flowcontrol, + uint32_t timeout_usec); + + // Destructor closes the serial port. + ~SerialStream() override; + + // Connect: Opens and configures the serial port. + // Returns true if port is successfully opened/already open. + // Returns false on opening/configuration failure. + bool Connect() override; + + // Disconnect: Closes the serial port. + // Returns true if port is successfully closed/already closed. + bool Disconnect() override; + + // Read data from the serial port. + // If timeout_usec > 0, waits up to timeout_usec for data. + // If timeout_usec == 0, returns immediately (non-blocking). + // Returns number of bytes read on success (> 0). + // Returns 0 on timeout or non-blocking with no data (EAGAIN/EWOULDBLOCK). + // Returns 0 if device not open and auto_reconnect fails or is disabled. + // Throws std::runtime_error for fatal read errors (other than + // EAGAIN/EWOULDBLOCK/EINTR) or if remote closed/device removed detected. + size_t read(uint8_t* buffer, size_t max_length) override; + + // Write data to the serial port. + // If timeout_usec > 0, waits up to timeout_usec for write buffer space if + // needed. If timeout_usec == 0, returns immediately (non-blocking) if cannot + // write. Returns number of bytes written on success (<= length). Returns 0 if + // device not open and auto_reconnect fails or is disabled. Throws + // std::runtime_error for fatal write errors (other than + // EAGAIN/EWOULDBLOCK/EINTR). + size_t write(const uint8_t* data, size_t length) override; + + private: + // Default constructor deleted to prevent instantiation without parameters. + SerialStream() = delete; + + // Helper function to open the serial port file descriptor. + // Throws std::runtime_error on failure. + void open(); + + // Helper function to close the serial port. Safe to call multiple times. + void close(); + + // Helper function to configure the serial port using termios. + // Assumes fd_ is valid and the port is open. + // Throws std::runtime_error on failure. + void configure_port(); + + // Waits for the file descriptor to become readable. + // Uses pselect with the given timeout. + // Returns true if readable, false on timeout or error. + // Sets last_errno_ on select error or timeout. + bool wait_readable(uint32_t timeout_us); + + // Waits for the file descriptor to become writable. + // Uses pselect with the given timeout. + // Returns true if writable, false on timeout or error. + // Sets last_errno_ on select error or timeout. + bool wait_writable(uint32_t timeout_us); + + // Heuristic check to detect if the device has been removed. + // Attempts a zero-byte write. If certain errors occur (EBADF, EIO), + // logs, disconnects, and attempts to reconnect. + // Returns true if device likely removed and disconnected. + // Returns false otherwise. + bool check_remove(); + + private: + // Stored configuration parameters + std::string device_name_; + baud_rate_t baud_rate_; + SerialByteSize bytesize_; + SerialParity parity_; + SerialStopBits stopbits_; + SerialFlowControl flowcontrol_; + uint32_t timeout_usec_; + + // Calculated time to transmit one byte (used potentially for write timeouts) + // Correctly calculated based on actual baud rate and settings. + uint32_t byte_time_us_ = 0; + + // File descriptor for the serial port. -1 indicates not open. + int fd_ = -1; + + // Indicates if the port is currently open. Kept for clarity alongside fd_. + bool is_open_ = false; +}; + +} // namespace hal +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/hal/stream/stream.h b/modules/drivers/hal/stream/stream.h new file mode 100644 index 00000000000..6247ff53a93 --- /dev/null +++ b/modules/drivers/hal/stream/stream.h @@ -0,0 +1,179 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * 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. + *****************************************************************************/ + +#pragma once + +#include +#include +#include + +#include "cyber/cyber.h" + +namespace apollo { +namespace drivers { +namespace hal { + +// An abstract class of Stream. +class Stream { + public: + // ========================================================================= + // Stream Class Overview and Responsibilities + // ========================================================================= + // This class serves as an abstract base class defining the interface + // for various stream types (e.g., serial, network). + // + // Responsibilities of Derived Classes: + // 1. Implement the pure virtual methods (Connect, Disconnect, read, write) + // according to the specific stream type. + // 2. **Crucially, derived classes MUST update the 'status_' member** + // based on the results of connection attempts and read/write operations. + // - Set status_ to CONNECTED after a successful Connect. + // - Set status_ to DISCONNECTED after a successful Disconnect or clean + // closure. + // - Set status_ to ERROR if a non-recoverable error occurs during any + // operation. + // + // Thread Safety: + // By default, instances of this class and its derived classes are NOT + // thread-safe. If a single Stream object is accessed concurrently by + // multiple threads, external synchronization (e.g., mutexes) is required, + // or the derived class must provide its own internal synchronization. + // ========================================================================= + virtual ~Stream() {} + + // Stream status. + enum class Status { + DISCONNECTED = 0, + CONNECTED, + ERROR, + }; + + static constexpr size_t NUM_STATUS = + static_cast(Stream::Status::ERROR) + 1; + + Status get_status() const { return status_; } + + // Attempts to connect the stream. + // Derived classes must implement this method. + // Returns true if connection was successful, false otherwise. + // **Derived classes should update status_ to CONNECTED on success + // or ERROR/DISCONNECTED on failure.** + virtual bool Connect() = 0; + + // Attempts to disconnect the stream. + // Derived classes must implement this method. + // Returns true if disconnection was successful, false otherwise. + // **Derived classes should update status_ to DISCONNECTED on success.** + virtual bool Disconnect() = 0; + + // Registers data required for the login process. + // This data is copied internally. + void RegisterLoginData(const std::vector &login_data) { + login_data_.assign(login_data.begin(), login_data.end()); + } + + // Executes the login process by writing registered data sequentially + // with delays. This method assumes a simple write-based login protocol. + // + // **This method is BLOCKING** due to the internal `cyber::Duration().Sleep()` + // calls and the sequential nature of writing login data. + // + // Derived classes' `write` method must be correctly implemented for this + // login logic to function. + // + // Returns true if all login data chunks were successfully written to the + // stream buffer. Returns false if the stream is not connected, or if any + // `write` operation fails or is incomplete for a login data chunk. + // + // Note: Success here only guarantees the data was sent to the stream + // interface. It does NOT guarantee that the remote end processed the login + // data successfully (which might require reading responses, not handled in + // this base method). + bool Login() { + if (status_ != Status::CONNECTED) { + AERROR << "Login failed: Stream is not CONNECTED. Current status: " + << static_cast(status_); + return false; + } + + for (size_t i = 0; i < login_data_.size(); ++i) { + const auto &data = login_data_[i]; + size_t bytes_written = write(data); + + AINFO << "Login step " << (i + 1) << "/" << login_data_.size() + << ": Attempted to write " << data.size() << " bytes, wrote " + << bytes_written << " bytes."; + + if (bytes_written != data.size()) { + // Error or partial write during this step of login + AERROR << "Login failed at step " << (i + 1) + << ": Could not write complete data chunk."; + return false; // Abort login sequence on first write failure + } + + // sleep a little to avoid overrun of the slow serial interface. + cyber::Duration(0.5).Sleep(); + } + + // All data chunks successfully written + return true; + } + + // Reads up to max_length bytes into the buffer. + // Derived classes must implement this method. + // Returns the number of bytes actually read (0 if no data available or + // error). + // **Derived classes should update status_ to ERROR or DISCONNECTED if a fatal + // read error occurs.** + virtual size_t read(uint8_t *buffer, size_t max_length) = 0; + + // Writes up to length bytes from the buffer. + // Derived classes must implement this method. + // Returns the number of bytes successfully written. + // A return value less than 'length' or 0 indicates a partial write or + // failure. + // **Derived classes should update status_ to ERROR or DISCONNECTED if a fatal + // write error occurs.** + virtual size_t write(const uint8_t *buffer, size_t length) = 0; + + size_t write(const std::string &buffer) { + return write(reinterpret_cast(buffer.data()), + buffer.size()); + } + + // Get the system errno of the last significant error. + // Returns 0 if no recent error or error was EAGAIN/EINTR. + int get_last_error_code() const { return last_errno_; } + + protected: + Stream() {} + + // The current status of the stream. + // Derived classes are responsible for keeping this updated + // based on the results of Connect, Disconnect, read, and write. + Status status_ = Status::DISCONNECTED; + + int last_errno_ = 0; + + private: + std::vector login_data_; + + DISALLOW_COPY_AND_ASSIGN(Stream); +}; + +} // namespace hal +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/hal/stream/stream_factory.cc b/modules/drivers/hal/stream/stream_factory.cc new file mode 100644 index 00000000000..6dc122bc6a3 --- /dev/null +++ b/modules/drivers/hal/stream/stream_factory.cc @@ -0,0 +1,87 @@ +// Copyright 2025 WheelOS. All Rights Reserved. +// +// 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. + +// Created Date: 2025-03-21 +// Author: daohu527 + +#include "modules/drivers/hal/stream/stream_factory.h" + +#include + +namespace apollo { +namespace drivers { +namespace hal { + +speed_t get_serial_baudrate(uint32_t rate) { + switch (rate) { + case 9600: + return B9600; + case 19200: + return B19200; + case 38400: + return B38400; + case 57600: + return B57600; + case 115200: + return B115200; + case 230400: + return B230400; + case 460800: + return B460800; + case 921600: + return B921600; + default: + return 0; + } +} + +StreamFactory::StreamPtr StreamFactory::CreateCan(const std::string& address, + uint32_t timeout_usec, + bool enable_can_fd) { + return std::make_unique(address, timeout_usec, enable_can_fd); +} + +StreamFactory::StreamPtr StreamFactory::CreateNtrip( + const std::string& address, uint16_t port, const std::string& mountpoint, + const std::string& user, const std::string& passwd, uint32_t timeout_s) { + return std::make_unique(address, port, mountpoint, user, passwd, + timeout_s); +} + +StreamFactory::StreamPtr StreamFactory::CreateSerial( + const std::string& device_name, uint32_t baud_rate, SerialByteSize bytesize, + SerialParity parity, SerialStopBits stopbits, SerialFlowControl flowcontrol, + uint32_t timeout_usec) { + speed_t baud = get_serial_baudrate(baud_rate); + return baud == 0 ? nullptr + : std::make_unique(device_name, baud, bytesize, + parity, stopbits, + flowcontrol, timeout_usec); +} + +StreamFactory::StreamPtr StreamFactory::CreateTcp(const std::string& address, + uint16_t port, + uint32_t timeout_usec) { + return std::make_unique(address, port, timeout_usec); +} + +StreamFactory::StreamPtr StreamFactory::CreateUdp(const std::string& address, + uint16_t port, + uint32_t timeout_usec) { + return std::make_unique(address, port, timeout_usec); +} + +} // namespace hal +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/hal/stream/stream_factory.h b/modules/drivers/hal/stream/stream_factory.h new file mode 100644 index 00000000000..829e7287570 --- /dev/null +++ b/modules/drivers/hal/stream/stream_factory.h @@ -0,0 +1,64 @@ +// Copyright 2025 WheelOS. All Rights Reserved. +// +// 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. + +// Created Date: 2025-03-21 +// Author: daohu527 + +#pragma once + +#include "modules/drivers/hal/stream/can_stream.h" +#include "modules/drivers/hal/stream/ntrip_stream.h" +#include "modules/drivers/hal/stream/serial_stream.h" +#include "modules/drivers/hal/stream/stream.h" +#include "modules/drivers/hal/stream/tcp_stream.h" +#include "modules/drivers/hal/stream/udp_stream.h" + +namespace apollo { +namespace drivers { +namespace hal { + +class StreamFactory { + public: + using StreamPtr = std::unique_ptr; + + static StreamPtr CreateTcp(const std::string &address, uint16_t port, + uint32_t timeout_usec = 1000000); + + static StreamPtr CreateUdp(const std::string &address, uint16_t port, + uint32_t timeout_usec = 1000000); + + // Currently the following baud rates are supported: + // 9600, 19200, 38400, 57600, 115200, 230400, 460800, 921600. + static StreamPtr CreateSerial( + const std::string &device_name, uint32_t baud_rate, + SerialByteSize bytesize = SerialByteSize::B8, + SerialParity parity = SerialParity::None, + SerialStopBits stopbits = SerialStopBits::One, + SerialFlowControl flowcontrol = SerialFlowControl::None, + uint32_t timeout_usec = 0); + + static StreamPtr CreateNtrip(const std::string &address, uint16_t port, + const std::string &mountpoint, + const std::string &user, + const std::string &passwd, + uint32_t timeout_s = 30); + + static StreamPtr CreateCan(const std::string &address, + uint32_t timeout_usec = 30 * 1000000u, + bool enable_can_fd = false); +}; + +} // namespace hal +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/hal/stream/tcp_stream.cc b/modules/drivers/hal/stream/tcp_stream.cc new file mode 100644 index 00000000000..3c32b047d49 --- /dev/null +++ b/modules/drivers/hal/stream/tcp_stream.cc @@ -0,0 +1,540 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * 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 "modules/drivers/hal/stream/tcp_stream.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "cyber/cyber.h" + +namespace apollo { +namespace drivers { +namespace hal { + +TcpStream::TcpStream(const std::string& address, uint16_t port, + uint32_t timeout_usec, bool auto_reconnect) + : sockfd_(-1), + timeout_usec_(timeout_usec), + auto_reconnect_(auto_reconnect) { + struct in_addr addr_struct; + int rc = inet_pton(AF_INET, address.c_str(), &addr_struct); + if (rc <= 0) { + // inet_pton returns 0 for invalid format, -1 for error and sets errno. + if (rc == 0) { + throw std::invalid_argument("Invalid IPv4 address format: " + address); + } else { // rc == -1 + // Check errno for system errors (less common for pton unless args + // invalid) + throw std::runtime_error("inet_pton failed for address " + address + + ": " + std::string(strerror(errno))); + } + } + // Store the converted address (which is in network byte order) + peer_addr_ = addr_struct.s_addr; + + // Convert port to network byte order + peer_port_ = htons(port); +} + +TcpStream::~TcpStream() { this->close(); } + +void TcpStream::open() { + if (sockfd_ >= 0) { + // Already open + return; + } + + int fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP); + if (fd < 0) { + last_errno_ = errno; + throw std::runtime_error("Failed to create TCP socket: " + + std::string(strerror(last_errno_))); + } + sockfd_ = fd; +} + +bool TcpStream::InitSocket() { + if (sockfd_ < 0) { + // Should not happen if called correctly after open(), but defensive check + throw std::runtime_error("InitSocket called with invalid sockfd_"); + } + + // Store original flags to restore after non-blocking connect + int original_flags = fcntl(sockfd_, F_GETFL, 0); + if (original_flags == -1) { + last_errno_ = errno; + ::close(sockfd_); + sockfd_ = -1; + throw std::runtime_error( + "fcntl(F_GETFL) failed for socket configuration: " + + std::string(strerror(last_errno_))); + } + + // Configure blocking/non-blocking and timeouts based on timeout_usec_ + if (timeout_usec_ > 0) { // Blocking with timeout + // Ensure blocking mode (remove O_NONBLOCK if present) + if (fcntl(sockfd_, F_SETFL, original_flags & ~O_NONBLOCK) == -1) { + last_errno_ = errno; + ::close(sockfd_); + sockfd_ = -1; + throw std::runtime_error( + "fcntl(F_SETFL, blocking) failed for socket configuration: " + + std::string(strerror(last_errno_))); + } + + // Set receive and send timeouts + struct timeval block_to = { + static_cast(timeout_usec_ / 1000000), // seconds + static_cast(timeout_usec_ % 1000000) // microseconds + }; + + if (setsockopt(sockfd_, SOL_SOCKET, SO_RCVTIMEO, &block_to, + sizeof(block_to)) < 0) { + last_errno_ = errno; + ::close(sockfd_); + sockfd_ = -1; + throw std::runtime_error("setsockopt(SO_RCVTIMEO) failed: " + + std::string(strerror(last_errno_))); + } + + if (setsockopt(sockfd_, SOL_SOCKET, SO_SNDTIMEO, &block_to, + sizeof(block_to)) < 0) { + last_errno_ = errno; + ::close(sockfd_); + sockfd_ = -1; + throw std::runtime_error("setsockopt(SO_SNDTIMEO) failed: " + + std::string(strerror(last_errno_))); + } + } else { // Non-blocking (timeout_usec_ == 0) + // Set non-blocking mode + if (fcntl(sockfd_, F_SETFL, original_flags | O_NONBLOCK) == -1) { + last_errno_ = errno; + ::close(sockfd_); + sockfd_ = -1; + throw std::runtime_error( + "fcntl(F_SETFL, non-blocking) failed for socket configuration: " + + std::string(strerror(last_errno_))); + } + // Note: SO_RCVTIMEO/SO_SNDTIMEO are not needed for pure non-blocking. + // EAGAIN/EWOULDBLOCK return codes indicate immediate operation would block. + } + + // Disable Nagle's algorithm (TCP_NODELAY) + int enable = 1; + if (setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, &enable, sizeof(enable)) == + -1) { + last_errno_ = errno; + // Nagle disable failing is often non-fatal, just log warning. + // Consider throwing if this is critical for the application. + AERROR << "setsockopt disable Nagle failed, errno: " << last_errno_ << ", " + << strerror(last_errno_) << ". Continuing."; + // Don't throw here unless disabling Nagle is a strict requirement. + } + + return true; // Configuration successful +} + +void TcpStream::close() { + if (sockfd_ >= 0) { + AINFO << "Closing TCP socket fd: " << sockfd_; + if (::close(sockfd_) < 0) { + last_errno_ = errno; + AERROR << "Failed to close TCP socket fd " << sockfd_ << ": " + << strerror(last_errno_); + } + sockfd_ = -1; + } +} + +bool TcpStream::Reconnect() { + if (!auto_reconnect_) { + return false; + } + + Disconnect(); // Close current socket gracefully + try { + if (Connect()) { + AINFO << "Reconnect tcp success."; + return true; + } + } catch (const std::exception& e) { + // Catch exception from Connect and log it as reconnection failure. + AERROR << "Reconnect failed (Connect threw): " << e.what(); + } + + return false; +} + +bool TcpStream::Connect() { + if (sockfd_ >= 0) { + // Already open + // Assuming status_ is correctly CONNECTED if sockfd_ >= 0 + return true; + } + + last_errno_ = 0; // Reset error code for this attempt + + try { + // 1. Create the socket + this->open(); // Throws on failure + + // 2. Set socket to non-blocking temporarily for connection attempt + int original_flags = fcntl(sockfd_, F_GETFL, 0); + if (original_flags == -1) { + last_errno_ = errno; + ::close(sockfd_); + sockfd_ = -1; + throw std::runtime_error("fcntl(F_GETFL) failed before connect: " + + std::string(strerror(last_errno_))); + } + if (fcntl(sockfd_, F_SETFL, original_flags | O_NONBLOCK) == -1) { + last_errno_ = errno; + ::close(sockfd_); + sockfd_ = -1; + throw std::runtime_error( + "fcntl(F_SETFL, non-blocking) failed before connect: " + + std::string(strerror(last_errno_))); + } + + // 3. Initiate non-blocking connect + struct sockaddr_in peer_sockaddr = {}; // Initialize to zeros + peer_sockaddr.sin_family = AF_INET; + peer_sockaddr.sin_port = peer_port_; // Network byte order + peer_sockaddr.sin_addr.s_addr = peer_addr_; // Network byte order + + int ret = ::connect(sockfd_, reinterpret_cast(&peer_sockaddr), + sizeof(peer_sockaddr)); + + if (ret < 0) { + if (errno == EINTR) { + AINFO << "Tcp connect returned EINTR during initial call. Retrying or " + "waiting."; + // EINTR during initial connect is rare but possible, treat as need to + // wait. + } else if (errno == EISCONN || errno == EALREADY) { + // Already connected (shouldn't happen if sockfd_ < 0 check passed, but + // defensive) + AINFO << "Tcp connect reported already connected."; + ret = 0; // Treat as success for flow below + } else if (errno != EINPROGRESS) { + // Immediate error other than EINPROGRESS + last_errno_ = errno; + AERROR << "Tcp connect failed immediately, error: " + << strerror(last_errno_); + ::close(sockfd_); + sockfd_ = -1; // Clean up socket + status_ = Stream::Status::ERROR; + return false; // Connection failed + } + // If errno == EINPROGRESS or EINTR, proceed to select wait + } + + // If ret == 0 initially, or errno was EINPROGRESS/EINTR, wait for + // connection completion + if (ret < 0 && (errno == EINPROGRESS || errno == EINTR)) { + // 4. Wait for connect to complete using select + fd_set writefds; + FD_ZERO(&writefds); + FD_SET(sockfd_, &writefds); + + struct timeval select_timeout; + if (timeout_usec_ == 0) { + // Non-blocking connect, no wait (select timeout 0) + select_timeout = {0, 0}; + AINFO << "Non-blocking connect, checking status immediately."; + } else { + // Blocking connect with timeout, use timeout_usec_ for select + select_timeout = {static_cast(timeout_usec_ / 1000000), + static_cast(timeout_usec_ % 1000000)}; + AINFO << "Waiting for connect with timeout: " << timeout_usec_ + << " us."; + } + + ret = select(sockfd_ + 1, NULL, &writefds, NULL, &select_timeout); + + if (ret < 0) { + last_errno_ = errno; + AERROR << "Wait connect failed (select error): " + << strerror(last_errno_); + ::close(sockfd_); + sockfd_ = -1; + status_ = Stream::Status::ERROR; + return false; // Connection failed + } else if (ret == 0) { + // select timed out + last_errno_ = ETIMEDOUT; // Use ETIMEDOUT to indicate connect timeout + AINFO << "Tcp connect timeout."; + ::close(sockfd_); + sockfd_ = -1; + status_ = Stream::Status::DISCONNECTED; + return false; // Connection failed + } else if (FD_ISSET(sockfd_, &writefds)) { + // Socket is writable, check for connection errors using SO_ERROR + int socket_error = 0; + socklen_t len = sizeof(socket_error); + if (getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &socket_error, &len) < + 0) { + last_errno_ = errno; + AERROR << "getsockopt(SO_ERROR) failed after select: " + << strerror(last_errno_); + ::close(sockfd_); + sockfd_ = -1; + status_ = Stream::Status::ERROR; + return false; // Connection failed + } + + if (socket_error != 0) { + // Connection failed asynchronously + last_errno_ = socket_error; // Use the actual socket error + AERROR << "Socket error after connect select: " + << strerror(last_errno_); + ::close(sockfd_); + sockfd_ = -1; // Clean up + status_ = Stream::Status::ERROR; + return false; // Connection failed + } + + // If we reach here, non-blocking connect completed successfully + AINFO << "Non-blocking connect completed successfully."; + + } else { + // This case should ideally not be reached if select returns > 0 but + // FD_ISSET is false + last_errno_ = EIO; // Indicate unexpected I/O error + AERROR + << "Select returned > 0 but FD_ISSET is false. Unexpected state."; + ::close(sockfd_); + sockfd_ = -1; + status_ = Stream::Status::ERROR; + return false; // Connection failed + } + } + + // 5. Restore original socket flags (blocking/non-blocking as per + // timeout_usec_) + if (fcntl(sockfd_, F_SETFL, original_flags) == -1) { + last_errno_ = errno; + // Failing to restore flags is bad, but connection was successful. Log and + // continue. + AERROR << "Failed to restore socket flags after connect. Connection " + "might behave unexpectedly: " + << strerror(last_errno_); + // Decide if this should be a fatal error requiring close/throw. + // For now, log and continue assuming connection is primary goal. + } + + // 6. Initialize other socket options (SO_RCVTIMEO, SO_SNDTIMEO, TCP_NODELAY + // etc.) InitSocket() handles cleanup if it fails. + InitSocket(); // Throws on failure + + // If we reach here, connection and configuration are successful + AINFO << "Tcp connect success to " << inet_ntoa({peer_addr_}) << ":" + << ntohs(peer_port_); + + status_ = Stream::Status::CONNECTED; + return true; // Connection successful + } catch (const std::exception& e) { + // Catch exceptions thrown by open() or InitSocket() + AERROR << "Failed to Connect (initial setup or config threw): " << e.what(); + // Socket might already be closed by the throwing helper, but ensure it. + if (sockfd_ >= 0) { + ::close(sockfd_); + sockfd_ = -1; + } + return false; // Connection failed + } +} + +bool TcpStream::Disconnect() { + if (sockfd_ < 0) { + // not open + return true; + } + + this->close(); + last_errno_ = 0; + status_ = Stream::Status::DISCONNECTED; + return true; +} + +size_t TcpStream::read(uint8_t* buffer, size_t max_length) { + if (sockfd_ < 0) { + // last_errno_ might hold previous error. + if (auto_reconnect_ && Reconnect()) { + // Successfully reconnected, proceed with read attempt + AINFO << "Read called on disconnected socket, reconnected successfully."; + } else { + // Not open, cannot reconnect, or reconnect failed. + last_errno_ = ENOTCONN; // Use ENOTCONN to indicate not connected + return 0; // Return 0 as nothing was read + } + } + + if (buffer == nullptr || max_length == 0) { + return 0; // Nothing to read into + } + + ssize_t ret; + // Use a loop to handle EINTR (interrupted system calls) + do { + ret = ::recv(sockfd_, buffer, max_length, 0); + } while (ret < 0 && errno == EINTR); + + if (ret < 0) { + // Handle errors other than EINTR + if (errno == EAGAIN || errno == EWOULDBLOCK) { + // Timeout or non-blocking socket has no data available + // Return 0 bytes read, which is standard for timeout/would block + // last_errno_ = errno; // Optionally store EAGAIN/EWOULDBLOCK + return 0; + } else { + // Other serious errors (e.g., socket closed, network error, permissions) + last_errno_ = errno; + AERROR << "TCP read error: " << strerror(last_errno_) + << " (errno: " << last_errno_ << "), fd: " << sockfd_; + // Fatal error, throw exception to indicate failure + // Caller needs to handle this and decide whether to Reconnect. + Disconnect(); // Clean up the broken socket + throw std::runtime_error("TCP read fatal error: " + + std::string(strerror(last_errno_))); + } + } + + if (ret == 0) { + // Remote closed the connection gracefully + AINFO << "TCP remote closed connection on fd: " << sockfd_; + last_errno_ = 0; // No system error code for remote closed + Disconnect(); // Clean up the socket + + // Attempt reconnect automatically if configured + if (auto_reconnect_ && Reconnect()) { + AINFO << "Read encountered remote close, reconnected successfully."; + // Return 0 bytes read for this attempt, caller can retry read. + return 0; + } else { + // Remote closed and auto-reconnect failed or disabled. + // Throw exception to signal the caller that the connection is lost. + throw std::runtime_error("TCP remote closed connection."); + } + } + + return ret; +} + +size_t TcpStream::write(const uint8_t* buffer, size_t length) { + if (sockfd_ < 0) { + // last_errno_ might hold previous error. + if (auto_reconnect_ && Reconnect()) { + // Successfully reconnected, proceed with write attempt + AINFO << "Write called on disconnected socket, reconnected successfully."; + } else { + // Not open, cannot reconnect, or reconnect failed. + last_errno_ = ENOTCONN; // Use ENOTCONN to indicate not connected + return 0; // Return 0 as nothing was written + } + } + + if (buffer == nullptr || length == 0) { + return 0; // Nothing to write + } + + size_t total_nsent = 0; + const uint8_t* current_data = buffer; + size_t remaining_length = length; + + // TCP send can write less than requested, so loop is necessary. + while (remaining_length > 0) { + ssize_t nsent; + // Use a loop for EINTR + do { + nsent = ::send(sockfd_, current_data, remaining_length, 0); + } while (nsent < 0 && errno == EINTR); + + if (nsent < 0) { + // Handle errors other than EINTR + last_errno_ = errno; + if (errno == EAGAIN || errno == EWOULDBLOCK) { + // Timeout or non-blocking socket cannot send immediately + // Return total bytes sent so far. Caller should retry sending the rest. + AINFO << "TCP write would block (EAGAIN/EWOULDBLOCK) after sending " + << total_nsent << " bytes."; + return total_nsent; // Return partial write count + indicate block + } else { + // Other serious errors (e.g., broken pipe, connection reset, network + // error) + AERROR << "TCP write error: " << strerror(last_errno_) + << " (errno: " << last_errno_ << "), fd: " << sockfd_; + // Assuming status_ is inherited. Set error status. + // status_ = Stream::Status::ERROR; // Or DISCONNECTED for pipe/reset + // errors Fatal error, throw exception. Caller needs to handle this and + // potentially Reconnect. + Disconnect(); // Clean up the broken socket + throw std::runtime_error("TCP write fatal error: " + + std::string(strerror(last_errno_))); + } + } + + // nsent >= 0 : Bytes were sent (could be 0 if length was 0, or partial). + total_nsent += nsent; + current_data += nsent; + remaining_length -= nsent; + } + + last_errno_ = 0; // Clear error on success + return total_nsent; +} + +bool TcpStream::Readable(uint32_t timeout_us) { + if (sockfd_ < 0) { + last_errno_ = ENOTCONN; + return false; + } + + // Setup a select call to block for serial data or a timeout + timespec timeout_ts; + fd_set readfds; + FD_ZERO(&readfds); + FD_SET(sockfd_, &readfds); + + timeout_ts.tv_sec = timeout_us / 1000000; + timeout_ts.tv_nsec = (timeout_us % 1000000) * 1000; + int r = pselect(sockfd_ + 1, &readfds, NULL, NULL, &timeout_ts, NULL); + if (r < 0) { + status_ = Stream::Status::ERROR; + last_errno_ = errno; + AERROR << "Failed to wait tcp data: " << errno << ", " << strerror(errno); + return false; + } else if (r == 0 || !FD_ISSET(sockfd_, &readfds)) { + return false; + } + // Data available to read. + return true; +} + +} // namespace hal +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/gnss/stream/tcp_stream.h b/modules/drivers/hal/stream/tcp_stream.h similarity index 50% rename from modules/drivers/gnss/stream/tcp_stream.h rename to modules/drivers/hal/stream/tcp_stream.h index fbac02894fe..b3a911c061f 100644 --- a/modules/drivers/gnss/stream/tcp_stream.h +++ b/modules/drivers/hal/stream/tcp_stream.h @@ -16,39 +16,59 @@ #pragma once +#include "modules/drivers/hal/stream/stream.h" + namespace apollo { namespace drivers { -namespace gnss { +namespace hal { class TcpStream : public Stream { - typedef uint16_t be16_t; - typedef uint32_t be32_t; + using ipv4_addr_t = uint32_t; + using port_t = uint16_t; public: - TcpStream(const char *address, uint16_t port, uint32_t timeout_usec, + TcpStream(const std::string& address, uint16_t port, uint32_t timeout_usec, bool auto_reconnect = true); - ~TcpStream(); + ~TcpStream() override; - virtual bool Connect(); - virtual bool Disconnect(); - virtual size_t read(uint8_t *buffer, size_t max_length); - virtual size_t write(const uint8_t *data, size_t length); + virtual bool Connect() override; + virtual bool Disconnect() override; + virtual size_t read(uint8_t* buffer, size_t max_length) override; + virtual size_t write(const uint8_t* data, size_t length) override; - private: - bool Reconnect(); bool Readable(uint32_t timeout_us); - TcpStream() {} + + // Attempts to disconnect and reconnect if auto_reconnect is enabled. + // Returns true on successful reconnection. + bool Reconnect(); + + private: + TcpStream() = delete; + // Helper function to create the socket file descriptor. + // Throws std::runtime_error on failure. void open(); + + // Helper function to close the socket. Safe to call multiple times. void close(); + + // Helper function to configure socket options (blocking/non-blocking, + // timeouts, TCP_NODELAY). bool InitSocket(); - be16_t peer_port_ = 0; - be32_t peer_addr_ = 0; - uint32_t timeout_usec_ = 0; + + private: + ipv4_addr_t peer_addr_ = 0; + port_t peer_port_ = 0; + + // File descriptor for the socket. -1 indicates not open. int sockfd_ = -1; - int errno_ = 0; + + // Configured timeout in microseconds. 0 indicates non-blocking. + uint32_t timeout_usec_ = 0; + + // Flag to enable automatic reconnection on read/write failures. bool auto_reconnect_ = false; }; -} // namespace gnss +} // namespace hal } // namespace drivers } // namespace apollo diff --git a/modules/drivers/hal/stream/udp_stream.cc b/modules/drivers/hal/stream/udp_stream.cc new file mode 100644 index 00000000000..2961e244923 --- /dev/null +++ b/modules/drivers/hal/stream/udp_stream.cc @@ -0,0 +1,260 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * 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 "modules/drivers/hal/stream/udp_stream.h" + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "cyber/cyber.h" + +namespace apollo { +namespace drivers { +namespace hal { + +UdpStream::UdpStream(const std::string& address, uint16_t port, + uint32_t timeout_usec) + : sockfd_(-1), timeout_usec_(timeout_usec) { + // Convert address string to network byte order binary form (IPv4) + struct in_addr addr_struct; + int rc = inet_pton(AF_INET, address.c_str(), &addr_struct); + + if (rc <= 0) { + // inet_pton returns 0 for invalid format, -1 for error and sets errno. + if (rc == 0) { + throw std::invalid_argument("Invalid IPv4 address format: " + address); + } else { + // rc == -1, check errno + throw std::runtime_error("inet_pton failed for address " + address + + ": " + strerror(errno)); + } + } + // Store the converted address (which is in network byte order) + peer_addr_ = addr_struct.s_addr; + + // Convert port to network byte order + peer_port_ = htons(port); +} + +UdpStream::~UdpStream() { this->close(); } + +void UdpStream::open() { + if (sockfd_ >= 0) { + // Already open, nothing to do + return; + } + + int fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); + if (fd < 0) { + last_errno_ = errno; + throw std::runtime_error("Failed to create UDP socket: " + + std::string(strerror(last_errno_))); + } + + // Configure blocking/non-blocking and timeouts based on timeout_usec_ + if (timeout_usec_ > 0) { // Blocking with timeout + int flags = fcntl(fd, F_GETFL, 0); + if (flags == -1) { + last_errno_ = errno; + ::close(fd); + throw std::runtime_error( + "fcntl(F_GETFL) failed for socket configuration: " + + std::string(strerror(last_errno_))); + } + + // Ensure blocking mode (remove O_NONBLOCK if present) + if (fcntl(fd, F_SETFL, flags & ~O_NONBLOCK) == -1) { + last_errno_ = errno; + ::close(fd); + throw std::runtime_error( + "fcntl(F_SETFL, blocking) failed for socket configuration: " + + std::string(strerror(last_errno_))); + } + + // Set receive and send timeouts + struct timeval block_to = { + static_cast(timeout_usec_ / 1000000), // seconds + static_cast(timeout_usec_ % 1000000) // microseconds + }; + + if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &block_to, sizeof(block_to)) < + 0) { + last_errno_ = errno; + ::close(fd); + throw std::runtime_error("setsockopt(SO_RCVTIMEO) failed: " + + std::string(strerror(last_errno_))); + } + + if (setsockopt(fd, SOL_SOCKET, SO_SNDTIMEO, &block_to, sizeof(block_to)) < + 0) { + last_errno_ = errno; + ::close(fd); + throw std::runtime_error("setsockopt(SO_SNDTIMEO) failed: " + + std::string(strerror(last_errno_))); + } + + } else { // Non-blocking (timeout_usec_ == 0) + int flags = fcntl(fd, F_GETFL, 0); + if (flags == -1) { + last_errno_ = errno; + ::close(fd); + throw std::runtime_error( + "fcntl(F_GETFL) failed for socket configuration: " + + std::string(strerror(last_errno_))); + } + + // Set non-blocking mode + if (fcntl(fd, F_SETFL, flags | O_NONBLOCK) == -1) { + last_errno_ = errno; + ::close(fd); + throw std::runtime_error( + "fcntl(F_SETFL, non-blocking) failed for socket configuration: " + + std::string(strerror(last_errno_))); + } + } + + // Socket is successfully created and configured + sockfd_ = fd; +} + +void UdpStream::close() { + if (sockfd_ >= 0) { // Check if socket is open + if (::close(sockfd_) < 0) { + // Log error but proceed, as sockfd_ must be invalidated anyway. + last_errno_ = errno; + AERROR << "Failed to close UDP socket fd " << sockfd_ << ": " + << strerror(last_errno_); + } + sockfd_ = -1; + } +} + +bool UdpStream::Connect() { + if (sockfd_ >= 0) { + return true; + } + + try { + // Attempt to open the socket. open() throws on failure. + this->open(); + status_ = Stream::Status::CONNECTED; + return true; + } catch (const std::exception& e) { + // Catch exceptions from open() and log the error. + AERROR << "Failed to Connect (open socket): " << e.what(); + status_ = Stream::Status::ERROR; + return false; + } +} + +bool UdpStream::Disconnect() { + if (sockfd_ < 0) { + return true; + } + + this->close(); + status_ = Stream::Status::DISCONNECTED; + return true; +} + +size_t UdpStream::read(uint8_t* buffer, size_t max_length) { + if (sockfd_ < 0) { + return 0; + } + + ssize_t ret = 0; + struct sockaddr_in peer_sockaddr; + socklen_t peer_len = sizeof(peer_sockaddr); + memset(&peer_sockaddr, 0, sizeof(peer_sockaddr)); + + // Use a loop to handle EINTR (interrupted system calls) + do { + ret = ::recvfrom(sockfd_, buffer, max_length, 0, + (struct sockaddr*)&peer_sockaddr, &peer_len); + } while (ret < 0 && errno == EINTR); + + if (ret < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK) { + // Timeout or non-blocking socket has no data available + // Return 0 bytes read, which is standard for timeout/would block + return 0; + } else { + // Other serious errors (e.g., socket closed, network error) + last_errno_ = errno; + AERROR << "UDP read error: " << strerror(last_errno_) + << " (errno: " << last_errno_ << "), fd: " << sockfd_; + return 0; + } + } + + return ret; +} + +size_t UdpStream::write(const uint8_t* data, size_t length) { + if (sockfd_ < 0) { + return 0; // Socket not open + } + + if (data == nullptr || length == 0) { + return 0; // Nothing to write + } + + struct sockaddr_in peer_sockaddr; + // Use memset for robustness, though individual assignments are also fine. + memset(&peer_sockaddr, 0, sizeof(peer_sockaddr)); + peer_sockaddr.sin_family = AF_INET; + peer_sockaddr.sin_port = peer_port_; + peer_sockaddr.sin_addr.s_addr = peer_addr_; + + socklen_t peer_len = sizeof(peer_sockaddr); + + // For UDP, sendto is usually atomic for datagrams within limits. + // A loop like in TCP write is generally not needed and might be misleading. + // Handle EINTR with a do-while loop. + ssize_t nsent; + do { + nsent = ::sendto(sockfd_, data, length, 0, (struct sockaddr*)&peer_sockaddr, + peer_len); + } while (nsent < 0 && errno == EINTR); + + if (nsent < 0) { + // Handle errors other than EINTR + if (errno == EAGAIN || errno == EWOULDBLOCK) { + // Timeout or non-blocking socket cannot send immediately + // Return 0 bytes sent + return 0; + } else { + // Other serious errors (e.g., network error, socket closed) + last_errno_ = errno; + AERROR << "UDP write error: " << strerror(last_errno_) + << " (errno: " << last_errno_ << "), fd: " << sockfd_; + return 0; + } + } + + return nsent; +} + +} // namespace hal +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/hal/stream/udp_stream.h b/modules/drivers/hal/stream/udp_stream.h new file mode 100644 index 00000000000..d50976dc1b0 --- /dev/null +++ b/modules/drivers/hal/stream/udp_stream.h @@ -0,0 +1,62 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * 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. + *****************************************************************************/ + +#pragma once + +#include "modules/drivers/hal/stream/stream.h" + +namespace apollo { +namespace drivers { +namespace hal { + +class UdpStream : public Stream { + using ipv4_addr_t = uint32_t; + using port_t = uint16_t; + + public: + UdpStream(const std::string& address, uint16_t port, uint32_t timeout_usec); + ~UdpStream() override; + + // Connect: For UDP, this conceptually means "open and configure the socket". + // It does NOT establish a connection like TCP. + // Returns true if socket is successfully opened/already open. + // Throws std::runtime_error if open fails. + virtual bool Connect() override; + virtual bool Disconnect() override; + virtual size_t read(uint8_t* buffer, size_t max_length) override; + virtual size_t write(const uint8_t* data, size_t length) override; + + private: + // Default constructor deleted to prevent instantiation without parameters. + UdpStream() = delete; + + // Helper function to create and configure the socket. + // Throws std::runtime_error on failure. + void open(); + + // Helper function to close the socket. Safe to call multiple times. + void close(); + + ipv4_addr_t peer_addr_ = 0; + port_t peer_port_ = 0; + uint32_t timeout_usec_ = 0; + int sockfd_ = -1; + int last_errno_ = 0; +}; + +} // namespace hal +} // namespace drivers +} // namespace apollo