// #include "RosInterface.hpp"

// #include <cv_bridge/cv_bridge.h>
// #include <ros/ros.h>
// #include <stdio.h>
// #include <stdlib.h>
// #include <time.h>

// #include <opencv2/core.hpp>
// #include <opencv2/highgui.hpp>

// namespace zone {

// namespace fusion_localization {

// namespace map {

// RosInterface::RosInterface(ros::NodeHandle& phpriv_nh, const std::string& prj_dir, const std::string& cfg_dir) {
//     mm_.Init(prj_dir, cfg_dir);
//     mm_.RegPubAheadMapCb([this](const IAheadMap& data) { RosInterface::PublishAheadMap(data); });
//     mm_.RegPubGnssFootLocCb([this](const IGnssFootLocation& data) { RosInterface::PublishGnssFootLoc(data); });

//     ros::NodeHandle node;

//     // 注册回调函数
//     // pic_sub_ = node.subscribe("/camera_array/cam2/image_raw/compressed", 10, &RosInterface::ReceivePic, this);

//     gnss_sub_ = node.subscribe("/roadsim_gps", 10, &RosInterface::ReceiveGnss, this);
//     eyeq4_data_sub_ = node.subscribe("/EyeQ4Parse", 10, &RosInterface::ReceiveEyeQ4Data, this);
//     rtk_sub_ = node.subscribe("/RtkData", 10, &RosInterface::ReceiveRtk, this);

//     // 设置发布
//     ahead_map_pub_ = phpriv_nh.advertise<map_msgs::AheadMap>("AheadMap", 10);
//     gnss_foot_pub_ = phpriv_nh.advertise<map_msgs::GnssFootLocation>("GnssFootLocation", 10);
// }

// RosInterface::~RosInterface() {}

// void RosInterface::ReceivePic(const sensor_msgs::CompressedImage::ConstPtr& msg) {
//     std::string filename = "/home/zhshuai/dev/tmp/pic/" + std::to_string(msg->header.seq) + "_" + std::to_string(msg->header.stamp.sec) + "_" + std::to_string(msg->header.stamp.nsec) + ".jpg";

//     cv::Mat mat0 = cv_bridge::toCvCopy(*msg, "bgr8")->image;
//     cv::imwrite(filename.c_str(), mat0);

//     return;
// }

// // void RosInterface::ReceiveGps(const roadsim::GNSS::ConstPtr& msg) {
// //     IGnssLocationData gnss_location_data = {};

// //     // gnss_location_data.timestamp.sec = msg->timestamp.data.sec;
// //     // gnss_location_data.timestamp.nanosec = msg->timestamp.data.nsec;
// //     gnss_location_data.gnssPoint.x = msg->gnss_longitude_;
// //     gnss_location_data.gnssPoint.y = msg->gnss_latitude_;
// //     gnss_location_data.gnssPoint.z = msg->gnss_height_;
// //     // gnss_location_data.heading = msg->heading;
// //     // gnss_location_data.speed = msg->speed;
// //     // gnss_location_data.coefficient = msg->coefficient;

// //     mm_.SaveGnssLocationData(gnss_location_data);

// //     return;
// // }

// void RosInterface::ReceiveGnss(const gnss_imu_msgs::GNSS::ConstPtr& msg) {
//     IGnssLocationData gnss_location_data = {};

//     gnss_location_data.timestamp.sec = msg->time_stamp_.time_stamp_s_;
//     gnss_location_data.timestamp.nanosec = msg->time_stamp_.time_stamp_ns_;
//     gnss_location_data.gnss_point.x = msg->gnss_longitude_;
//     gnss_location_data.gnss_point.y = msg->gnss_latitude_;
//     gnss_location_data.gnss_point.z = msg->gnss_height_;
//     gnss_location_data.heading = msg->gnss_heading_;
//     gnss_location_data.speed = msg->gnss_ground_speed_;
//     gnss_location_data.coefficient = msg->gnss_pitch_standard_deviation_;
//     if (msg->gnss_longitude_standard_deviation_ * msg->gnss_latitude_standard_deviation_ <= 0.01) {
//         gnss_location_data.credible = true;
//     } else {
//         gnss_location_data.credible = false;
//     }

//     mm_.SaveGnssLocationData(gnss_location_data);

//     return;
// }

// void RosInterface::ReceiveEyeQ4Data(const eyeq4_msgs::MeData::ConstPtr& msg) {
//     IVisionMap vision = {};

//     // time stamp
//     vision.timestamp.sec = msg->header.stamp.sec;
//     vision.timestamp.nanosec = msg->header.stamp.nsec;

//     // crossing
//     if (msg->lane_info_.left_crossing_) {
//         vision.crossing = MarkingCrossing::EDGE_CROSSING_LEFT_CROSSING;
//     } else if (msg->lane_info_.right_crossing_) {
//         vision.crossing = MarkingCrossing::EDGE_CROSSING_RIGHT_CROSSING;
//     } else {
//         vision.crossing = MarkingCrossing::EDGE_CROSSING_NO_CROSSING;
//     }

//     // lane marks
//     for (int i = 0; i < msg->lanes_mark_info_.size(); i++) {
//         const eyeq4_msgs::LanesMarkInfo& lane = msg->lanes_mark_info_[i];
//         // invalid type
//         if (0 == lane.line_type_) {
//             continue;
//         }
//         // lane edge
//         else if (1 == lane.line_type_) {
//             ILaneMarking lane_edge = {};
//             if (false == ConvertLanePos(lane.line_side_, lane_edge.pos)) {
//                 continue;
//             }
//             if (false == ConvertLaneColor(lane.line_color_, lane_edge.lane_marking_color)) {
//                 continue;
//             }
//             if (false == ConvertLaneCross(lane.line_dlm_type_, lane.line_lane_mark_type_, lane_edge.cross_type)) {
//                 continue;
//             }
//             lane_edge.angle = lane.line_c1_;
//             lane_edge.dis = lane.line_c0_;
//             lane_edge.exist_prob = lane.line_exist_prob_;
//             vision.lane_markings.push_back(lane_edge);
//         }
//         // road edge
//         else if (2 == lane.line_type_) {
//             ILinkMarking link_edge = {};
//             if (false == ConvertLinkPos(lane.line_side_, link_edge.pos)) {
//                 continue;
//             }
//             if (false == ConvertLaneCross(lane.line_dlm_type_, lane.line_lane_mark_type_, link_edge.type)) {
//                 continue;
//             }
//             link_edge.angle = lane.line_c1_;
//             link_edge.dis = lane.line_c0_;
//             link_edge.exist_prob = lane.line_exist_prob_;
//             if (LinkMarkingSide::LINK_MARKING_SIDE_LEFT == link_edge.pos) {
//                 vision.left_link_marking.push_back(link_edge);
//             } else {
//                 vision.right_link_marking.push_back(link_edge);
//             }
//         }
//         // invalid type
//         else {
//             continue;
//         }
//     }

//     if (true == vision.lane_markings.empty()) {
//         return;
//     }

//     if (vision.lane_markings.size() >= 2) {
//         int b = 0;
//     }

//     if (0 != msg->tsr_info_.tsr_shape_) {
//         ITrafsignMap trafsign = {};
//         trafsign.lat_dis = msg->tsr_info_.tsr_sign_lat_dis_;
//         trafsign.lon_dis = msg->tsr_info_.tsr_sign_long_dis_;
//         trafsign.traffic_sign_shape = msg->tsr_info_.tsr_shape_;
//         vision.traffic_signs.push_back(trafsign);
//     }

//     // Print the time of eyeq4 message
//     time_t rawtime = (time_t)(msg->com_header_.time_stamp_.time_stamp_s_);
//     struct tm* timeinfo = localtime(&rawtime);
//     std::string strTime = asctime(timeinfo);
//     ROS_INFO("RosInterface::ReceiveEyeQ4Data(), received a eyeq4 message, time: %s, left road edge = %d, lane edge = %d, right road edge = %d, tsr count = %d", strTime.substr(0, strTime.find_last_of("\n")).c_str(), vision.left_link_marking.size(),
//              vision.lane_markings.size(), vision.right_link_marking.size(), vision.traffic_signs.size());

//     mm_.SaveVisionMapData(vision);

//     return;
// }

// void RosInterface::ReceiveRtk(const map_msgs::RtkData::ConstPtr& msg) {}

// void RosInterface::PublishGnssFootLoc(const IGnssFootLocation& data) {
//     map_msgs::GnssFootLocation msg = {};

//     msg.header.stamp = ros::Time::now();

//     msg.change_lanes_ = data.change_lanes;
//     msg.lane_seq_no_ = data.lane_seq_no;
//     msg.lane_num_ = data.lane_num;
//     msg.road_type_ = data.road_type;
//     msg.angle_with_lane_ = data.angle_with_lane;
//     msg.road_type_begin_dis_ = data.road_type_begin_dis;
//     msg.road_type_end_dis_ = data.road_type_end_dis;
//     msg.in_road_type_ = data.in_road_type;
//     msg.left_lane_edge_dis_ = data.left_lane_edge_dis;
//     msg.right_lane_edge_dis_ = data.right_lane_edge_dis;
//     msg.foot_point_.x_ = data.foot_point.x;
//     msg.foot_point_.y_ = data.foot_point.y;
//     msg.foot_point_.z_ = data.foot_point.z;
//     msg.vision_horizontal_point_.x_ = data.vision_horizontal_point.x;
//     msg.vision_horizontal_point_.y_ = data.vision_horizontal_point.y;
//     msg.vision_horizontal_point_.z_ = data.vision_horizontal_point.z;
//     msg.vision_vertical_point_.x_ = data.vision_vertical_point.x;
//     msg.vision_vertical_point_.y_ = data.vision_vertical_point.y;
//     msg.vision_vertical_point_.z_ = data.vision_vertical_point.z;

//     gnss_foot_pub_.publish(msg);

//     return;
// }

// void RosInterface::PublishAheadMap(const IAheadMap& data) {
//     map_msgs::AheadMap msg = {};

//     msg.cur_lane_left_color_ = (int)data.cur_lane_left_color;
//     msg.cur_lane_right_color_ = (int)data.cur_lane_right_color;
//     msg.cur_lane_left_type_ = (int)data.cur_lane_left_type;
//     msg.cur_lane_right_type_ = (int)data.cur_lane_right_type;
//     msg.next_lane_left_color_ = (int)data.next_lane_left_color;
//     msg.next_lane_right_color_ = (int)data.next_lane_right_color;
//     msg.next_lane_left_type_ = (int)data.next_lane_left_type;
//     msg.next_lane_right_type_ = (int)data.next_lane_right_type;
//     msg.left_lane_geo_lon_ = data.left_lane_geo_lon;
//     msg.left_lane_geo_lat_ = data.left_lane_geo_lat;
//     msg.left_lane_geo_alt_ = data.left_lane_geo_alt;
//     msg.right_lane_geo_lon_ = data.right_lane_geo_lon;
//     msg.right_lane_geo_lat_ = data.right_lane_geo_lat;
//     msg.right_lane_geo_alt_ = data.right_lane_geo_alt;
//     for (auto ts : data.traffic_signs) {
//         map_msgs::Trafsign tsMsg = {};
//         tsMsg.traffic_sign_class_ = (int)ts.traffic_sign_class;
//         tsMsg.traffic_sign_sub_class_ = ts.traffic_sign_sub_class;
//         tsMsg.speed_limit_value_ = ts.speed_limit_value;
//         msg.traffic_signs_.push_back(tsMsg);
//     }

//     ahead_map_pub_.publish(msg);

//     return;
// }

// bool RosInterface::ConvertLanePos(int src, LaneMarkingSide& dst) {
//     switch (src) {
//         case 1:  //
//             dst = LaneMarkingSide::LANE_MARKING_SIDE_LEFT;
//             return true;
//         case 2:  //
//             dst = LaneMarkingSide::LANE_MARKING_SIDE_LEFT_LEFT;
//             return true;
//         case 3:  //
//             dst = LaneMarkingSide::LANE_MARKING_SIDE_RIGHT;
//             return true;
//         case 4:  //
//             dst = LaneMarkingSide::LANE_MARKING_SIDE_RIGHT_RIGHT;
//             return true;
//         default:
//             return false;
//     }

//     return false;
// }

// bool RosInterface::ConvertLaneColor(int src, HMC::LaneMarkingColor& dst) {
//     switch (src) {
//         case 1:
//             dst = HMC::LaneMarkingColor::HDM_LANE_MARKING_COLOR_WHITE;
//             return true;
//         case 2:  //
//             dst = HMC::LaneMarkingColor::HDM_LANE_MARKING_COLOR_YELLOW;
//             return true;
//         case 3:  //
//             dst = HMC::LaneMarkingColor::HDM_LANE_MARKING_COLOR_BLUE;
//             return true;
//         default:
//             return false;
//     };

//     return false;
// }

// bool RosInterface::ConvertLaneCross(int src_double, int src_single, HMC::LaneMarkingType& dst) {
//     if (0 != src_double) {
//         switch (src_double) {
//             case 1:  //
//                 dst = HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_LEFT_SOLID_RIGHT_DASHED;
//                 return true;
//             case 2:  //
//                 dst = HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_RIGHT_SOLID_LEFT_DASHED;
//                 return true;
//             case 3:  //
//                 dst = HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_DOUBLE_SOLID;
//                 return true;
//             case 4:  //
//                 dst = HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_DOUBLE_DASHED;
//                 return true;
//             default:
//                 return false;
//         };
//     } else {
//         switch (src_single) {
//             case 1:  //
//                 dst = HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_SINGLE_SOLID;
//                 return true;
//             case 2:  //
//                 dst = HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_SINGLE_DASHED;
//                 return true;
//             case 4:  //
//                 dst = HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_SHORT_THICK_DASHED;
//                 return true;
//             default:
//                 return false;
//         }
//     }

//     return false;
// }

// bool RosInterface::ConvertLinkPos(int src, LinkMarkingSide& dst) {
//     switch (src) {
//         case 1:  //
//             dst = LinkMarkingSide::LINK_MARKING_SIDE_LEFT;
//             return true;
//         case 3:  //
//             dst = LinkMarkingSide::LINK_MARKING_SIDE_RIGHT;
//             return true;
//         default:
//             return false;
//     }

//     return false;
// }

// }  // namespace map

// }  // namespace fusion_localization

// }  // namespace zone