Skip to content

M
map_location
Commit 2f27b72f authored by muyusheng's avatar muyusheng
Browse files
Options

Merge remote-tracking branch 'origin/master'

parents 2cb8e9e2 ae3f1dd2
Hide whitespace changes
Inline Side-by-side
Showing with 667 additions and 0 deletions
src/MapLocation.cpp
View file @ 2f27b72f

#include "MapLocation.h"
#include <sys/time.h>
#include <unistd.h>
#include "CoordSys.h"
namespace JfxLocation
{
static const double sc_min_prob = 0.8;
using namespace jf;
uint64_t GetCurTime()
{
struct timeval time;
gettimeofday(&time, NULL);
uint64_t seconds = time.tv_sec;
uint64_t ttt = seconds * 1000 * 1000 + time.tv_usec;
return ttt;
}
MapLocation::MapLocation(const std::string& project, const std::string& config)
{
jf::HdLib::GetInstance()->Init(project, config); //初始化地图
}
MapLocation::~MapLocation()
{
if (m_isRun == true)
{
m_isRun = false;
if (m_thread.joinable()) {
m_thread.join();
}
}
}
int MapLocation::SetMatchingResultCallback(MatchingResultCallback cb)
{
m_mmMatch.Init();
m_isRun = true;
m_thread = std::thread(&MapLocation::ThreadProcess, this);
std::lock_guard<std::mutex> lock(m_mtx);
m_cb = cb;
return 0;
}
void MapLocation::ThreadProcess()
{
uint64_t tip_time = GetCurTime();
while (m_isRun)
{
if (GetCurTime() - tip_time > 100 * 1000)
{
IMatchResult data = {};
if (true == m_mmMatch.GetMatchResultData(data))
{
MatchingResult result = {};
result.time_stamp = (int64_t)data.timestamp.sec * 1e6 + (int64_t)data.timestamp.nanosec / 1000;
result.foot_point.x = data.foot_point.x;
result.foot_point.y = data.foot_point.y;
result.foot_point.z = data.foot_point.z;
result.foot_prob = data.foot_prob;
result.vision_horizontal_point.x = data.vision_horizontal_point.x;
result.vision_horizontal_point.y = data.vision_horizontal_point.y;
result.vision_horizontal_point.z = data.vision_horizontal_point.z;
result.horizontal_prob = data.horizal_prob;
result.vision_vertical_point.x = data.vision_vertical_point.x;
result.vision_vertical_point.y = data.vision_vertical_point.y;
result.vision_vertical_point.z = data.vision_vertical_point.z;
result.vertical_prob = data.vertical_prob;
result.match_status = data.match_status;
result.change_lanes = (int8_t)data.change_lanes;
result.lane_seq_no = data.lane_seq_no;
result.lane_num = data.lane_num;
result.road_rank = data.road_rank;
result.road_type = data.road_type;
result.angle_with_lane = data.angle_with_lane;
result.special_type = data.special_type;
result.special_type_begin_dis = data.special_type_begin_dis;
result.special_type_end_dis = data.special_type_end_dis;
result.in_road_crossing = data.in_road_crossing;
result.left_lane_edge_dis = data.left_lane_edge_dis;
result.right_lane_edge_dis = data.right_lane_edge_dis;
result.gnss_time = data.gnss_time;
result.gnss_longitude = data.gnss_point.x;
result.gnss_latitude = data.gnss_point.y;
result.gnss_height = data.gnss_point.z;
result.gnss_heading = data.gnss_heading;
result.gnss_ground_speed = data.gnss_speed;
result.gnss_longitude_standard_deviation = data.gnss_longitude_deviation;
result.gnss_latitude_standard_deviation = data.gnss_latitude_deviation;
result.gnss_height_standard_deviation = data.gnss_height_deviation;
result.gnss_solution_status = data.gnss_solution_status;
std::lock_guard<std::mutex> lock(m_mtx);
if (m_cb)
m_cb(result);
}
tip_time += 100 * 1000;
}
else
{
int64_t sleep = tip_time + 100 * 1000 - GetCurTime();
if(sleep > 0)
usleep(sleep);
}
}
}
int MapLocation::PushGPSFrame(GPSFrame& gps)
{
IGnssLocationData gnss_location_data = {};
gnss_location_data.timestamp.sec = gps.time_stamp / 1e6;
gnss_location_data.timestamp.nanosec = (gps.time_stamp - gnss_location_data.timestamp.sec * 1e6) * 1000;
gnss_location_data.gnss_point.x = gps.longitude;
gnss_location_data.gnss_point.y = gps.latitude;
gnss_location_data.gnss_point.z = gps.altitude;
gnss_location_data.heading = gps.heading;
gnss_location_data.speed = gps.ground_speed;
gnss_location_data.coefficient = gps.pitch_deviation;
if (sqrt(pow(gps.longitude_deviation, 2) + pow(gps.latitude_deviation, 2)) <= 0.01) {
gnss_location_data.credible = true;
}
else {
gnss_location_data.credible = false;
}
gnss_location_data.credible = true;
// gnss_location_data.gnss_time = 315936000.0 - 18 + msg->gps_time_.gnss_week_ * 7 * 86400 + msg->gps_time_.seconds_from_week_start_ + 8 * 3600;
gnss_location_data.gnss_time = gps.jfx_gps_time;
gnss_location_data.longitude_deviation = gps.longitude_deviation;
gnss_location_data.latitude_deviation = gps.latitude_deviation;
gnss_location_data.height_deviation = gps.height_deviation;
gnss_location_data.solution_status = gps.solution_status;
m_mmMatch.SaveGnssLocData(gnss_location_data);
return 0;
}
bool ConvertLanePos(int src, LaneMarkingSide& dst)
{
switch (src) {
case 1: //
dst = LaneMarkingSide::LANE_MARKING_SIDE_LEFT;
return true;
case 2: // right
dst = LaneMarkingSide::LANE_MARKING_SIDE_RIGHT;
return true;
case 3: //
dst = LaneMarkingSide::LANE_MARKING_SIDE_LEFT_LEFT;
return true;
case 4: //
dst = LaneMarkingSide::LANE_MARKING_SIDE_RIGHT_RIGHT;
return true;
default:
return false;
}
return false;
}
bool 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:
dst = HMC::LaneMarkingColor::HDM_LANE_MARKING_COLOR_WHITE;
return false;
};
return false;
}
bool 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 3: //
return false;
case 4: //
dst = HMC::LaneMarkingType::HDM_LANE_MARKING_TYPE_SHORT_THICK_DASHED;
return true;
default:
return false;
}
}
return false;
}
bool ConvertLinkPos(int src, LinkMarkingSide& dst) {
switch (src) {
case 1: //
case 3: //
dst = LinkMarkingSide::LINK_MARKING_SIDE_LEFT;
return true;
case 2: //
case 4: //
dst = LinkMarkingSide::LINK_MARKING_SIDE_RIGHT;
return true;
default:
return false;
}
return false;
}
bool ConvertTsr(const Lines& src, ITrafsignMap& dst) {
switch (src.tsr_sign_name) {
case 0: // Regular 10
case 28: // Electronic 10
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 10;
break;
case 1: // Regular 20
case 29: // Electronic 20
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 20;
break;
case 2: // Regular 30
case 30: // Electronic 30
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 30;
break;
case 3: // Regular 40
case 31: // Electronic 40
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 40;
break;
case 4: // Regular 50
case 32: // Electronic 50
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 50;
break;
case 5: // Regular 60
case 33: // Electronic 60
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 60;
break;
case 6: // Regular 70
case 34: // Electronic 70
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 70;
break;
case 7: // Regular 80
case 35: // Electronic 80
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 80;
break;
case 8: // Regular 90
case 36: // Electronic 90
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 90;
break;
case 9: // Regular 100
case 37: // Electronic 100
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 100;
break;
case 10: // Regular 110
case 38: // Electronic 110
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 110;
break;
case 11: // Regular 120
case 39: // Electronic 120
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 120;
break;
case 12: // Regular 130
case 40: // Electronic 130
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 130;
break;
case 41: // Electronic 140
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 140;
case 23: // Car Limit
break;
case 25: // Minimum Speed Limits
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 14;
break;
case 26: // Motorcycle Limit
break;
case 79: // End of Speed 2 digits
break;
case 80: // End of Speed 3 digits
break;
case 100: // Regular 5
case 115: // Electronic 5
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 5;
break;
case 101: // Regular 15
case 116: // Electronic 15
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 15;
break;
case 102: // Regular 25
case 117: // Electronic 25
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 25;
break;
case 103: // Regular 35
case 118: // Electronic 35
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 35;
break;
case 104: // Regular 45
case 119: // Electronic 45
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 45;
break;
case 105: // Regular 55
case 120: // Electronic 55
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 55;
break;
case 106: // Regular 65
case 121: // Electronic 65
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 65;
break;
case 107: // Regular 75
case 122: // Electronic 75
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 75;
break;
case 108: // Regular 85
case 123: // Electronic 85
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 85;
break;
case 109: // Regular 95
case 124: // Electronic 95
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 95;
break;
case 110: // Regular 105
case 125: // Electronic 105
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 105;
break;
case 111: // Regular 115
case 126: // Electronic 115
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 115;
break;
case 112: // Regular 125
case 127: // Electronic 125
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 125;
break;
case 113: // Regular 135
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 135;
break;
case 114: // Regular 145
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 11;
dst.speed_limit_value = 145;
break;
case 152: // Right Merge
dst.traffic_sign_class = 1;
dst.traffic_sign_sub_class = 49;
break;
case 207: // No stopping
break;
case 256: // Bicycle Lane
break;
case 260: // Height Limit
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 8;
break;
case 266: // Weight Limit
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 9;
break;
case 267: // Width Limit
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 13;
break;
default:
break;
}
switch (src.tsr_sup1_sign_name) {
case 0: // None
break;
case 1: // Rain
break;
case 2: // Snow
break;
case 3: // Trailer
break;
case 4: // Time
break;
case 5: // Arrow_left
dst.traffic_sign_class = 4;
dst.traffic_sign_sub_class = 3;
case 6: // Arrow_right
dst.traffic_sign_class = 4;
dst.traffic_sign_sub_class = 4;
case 7: // BendArrow_left
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 5;
case 8: // BendArrow_right
dst.traffic_sign_class = 5;
dst.traffic_sign_sub_class = 6;
case 9: // Truck
break;
case 10: // Distance_arrow(distance_for)
break;
case 11: // Weight
break;
case 12: // Distance_in
break;
case 13: // Tractor
break;
case 14: // Snow_rain
break;
case 15: // School
dst.traffic_sign_class = 6;
break;
case 16: // Rain_cloud
break;
case 17: // Fog
break;
case 18: // Hazerdous_materials
break;
case 19: // Night
break;
case 20: // Supp_sign_generic
break;
case 21: // e_rappel
break;
case 22: // e_zone
break;
case 23: // Ramp
dst.traffic_sign_class = 2;
dst.traffic_sign_sub_class = 9;
dst.traffic_sign_sub_class = 10;
break;
case 24: // End
break;
case 25: // Exit
break;
case 26: // Reserved
break;
case 27: // Minimum
break;
case 34: // Arrow_bidirectional
break;
case 35: // Work_Zone
break;
case 255: //= Invalid_supp;
break;
default:
break;
}
return true;
}
int MapLocation::PushLines(Lines& lines)
{
/* 我们需要的数据
lanes_mark_count_ 车道线个数
lanes_mark_info_ 单个车道线信息
line_exist_prob_ 置信度
line_color_ 车道线颜色（最好有，没有也行）
line_dlm_type_ 车道线双线的类型
line_lane_mark_type_ 车道线单线的类型
line_c0_ 车离车道线距离
line_c1_ 车的方向与车道线的角度
line_side_ 车道线是左1，左2，右1，右2 中的一种
line_type_ 车道线是车道边界还是道路边界
lane_info_ 车道信息
left_crossing_ 是否是向左跨线
right_crossing_ 是否向右跨线
view_start_ 离车道线起始点的距离（单位：）
view_end_ 离车道线结束点的距离（单位：）
tsr_count_ 标牌数量
tsr_info_ 标牌信息
*/
IVisionMap vision = {};
// time stamp
vision.timestamp.sec = lines.time_stamp / 1e6;;
vision.timestamp.nanosec = (lines.time_stamp - vision.timestamp.sec * 1e6) * 1000;
// crossing
if (true == lines.left_crossing) {
vision.crossing = MarkingCrossing::EDGE_CROSSING_LEFT_CROSSING;
// ROS_WARN("RosInterface::ReceiveAduData(), left crossing");
}
else if (true == lines.right_crossing) {
vision.crossing = MarkingCrossing::EDGE_CROSSING_RIGHT_CROSSING;
// ROS_WARN("RosInterface::ReceiveAduData(), right crossing");
}
else {
vision.crossing = MarkingCrossing::EDGE_CROSSING_NO_CROSSING;
}
// lane marks
for (int i = 0; i < lines.lines.size(); i++)
{
Line& lane = lines.lines[i];
if (0 == lane.line_type) {
//ROS_WARN("RosInterface::ReceiveAduData(), invalid type: %d", lane.line_type_);
continue;
}
// lane edge
else if (1 == lane.line_type) {
if (lane.conf >= sc_min_prob && lane.line_c0 != 0) {
ILaneMarking lane_edge = {};
if (false == ConvertLanePos(lane.line_side, lane_edge.pos)) {
//ROS_WARN("RosInterface::ReceiveAduData(), invalid side type: %d", lane.line_side_);
continue;
}
if (false == ConvertLaneColor(lane.line_color, lane_edge.lane_marking_color)) {
//ROS_WARN("RosInterface::ReceiveAduData(), invalid color type: %d", lane.line_color_);
continue;
}
// lane_edge.color_prob = lane.line_color_prob_;
if (false == ConvertLaneCross(lane.line_dlm_type, lane.line_lane_mark_type, lane_edge.cross_type)) {
//ROS_WARN("RosInterface::ReceiveAduData(), invalid cross type: dlm = %d, mark = %d", lane.line_dlm_type_, lane.line_lane_mark_type_);
continue;
}
lane_edge.angle = lane.line_c1;
lane_edge.dis = lane.line_c0;
lane_edge.exist_prob = lane.conf * 100;
vision.lane_markings.push_back(lane_edge);
}
}
// road edge
else if (2 == lane.line_type) {
if (lane.conf >= sc_min_prob && lane.line_c0 != 0) {
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.conf;
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);
}
// ROS_WARN("RosInterface::ReceiveAduData(), time: %d seconds, link pos = %d, lane1 prob = %d:%d", msg->header.stamp - start_time, link_edge.pos, lane.line_color_prob_, lane.line_exist_prob_);
}
}
// invalid type
else {
//ROS_WARN("RosInterface::ReceiveAduData(), invalid type: %d", lane.line_type_);
continue;
}
}
if (true == vision.lane_markings.empty()) {
//ROS_WARN("RosInterface::ReceiveAduData(), no valid lane");
return 0;
}
if (0 != lines.tsr_shape) {
ITrafsignMap trafsign = {};
trafsign.lat_dis = lines.tsr_sign_lat_dis;
trafsign.lon_dis = lines.tsr_sign_long_dis;
trafsign.traffic_sign_shape = lines.tsr_shape;
trafsign.exist_prob = lines.tsr_prob;
vision.traffic_signs.push_back(trafsign);
ConvertTsr(lines, trafsign);
// ROS_WARN("RosInterface::ReceiveAduData(), time: %d seconds, shape = %d, lat dis = %f, long dis = %f, tsr prob = %d", msg->header.stamp - start_time, msg->tsr_info_.tsr_shape_, msg->tsr_info_.tsr_sign_lat_dis_,
// msg->tsr_info_.tsr_sign_long_dis_,
// msg->tsr_info_.tsr_prob_);
}
static int s_nRcvCnt = 0;
if (s_nRcvCnt % 4 == 0) {
m_mmMatch.SaveVisionMapData(vision);
}
s_nRcvCnt++;
return 0;
}
int MapLocation::PushLocation(Location& loc)
{
auto func_wraps2pi_pi = [](double& rad) {
while (rad > pi) rad -= 2 * pi;
while (rad < (-pi)) rad += 2 * pi;
return rad;
};
auto func_Quat2Euler = [&func_wraps2pi_pi](double q0, double q1, double q2, double q3, double& roll, double& pitch, double& heading) {
double gamma = atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2));
double theta = asin(2.0 * (q0 * q2 - q3 * q1));
double psi = atan2(2.0 * (q0 * q3 + q1 * q2), 1.0 - 2.0 * (q2 * q2 + q3 * q3));
roll = func_wraps2pi_pi(gamma);
pitch = func_wraps2pi_pi(theta);
heading = func_wraps2pi_pi(psi);
};
ILocalization loc_ = {};
loc_.lTimestamp = (double)loc.time_stamp / 1e6;
loc_.loc_point.x = loc.pose.position.x;
loc_.loc_point.y = loc.pose.position.y;
loc_.loc_point.z = loc.pose.position.z;
loc_.gnss_available = loc.gnss_available;
func_Quat2Euler(loc.pose.orientation.x, loc.pose.orientation.y, loc.pose.orientation.z, loc.pose.orientation.w, loc_.roll, loc_.pitch, loc_.heading);
loc_.heading = loc.attitude.yaw < 0 ? loc.attitude.yaw + 360 : loc.attitude.yaw;
loc_.speed = pow((pow(loc.velocity.linear_velocity.x, 2) + pow(loc.velocity.angular_velocity.y, 2)), 0.5);
loc_.longitude_accuracy = loc.acceleration.std_linear_acceleration.x;
loc_.latitude_accuracy = loc.acceleration.std_linear_acceleration.y;
// SendLocRes(msg, loc.speed);
// PublishLocCanFd(msg, loc.speed);
static int s_nRcvCnt2 = 0;
if (s_nRcvCnt2 % 10 == 0) {
m_mmMatch.SaveEkfResultData(loc_);
}
s_nRcvCnt2++;
return 0;
}
}
src/MapLocation.h
View file @ 2f27b72f
......@@ -3,6 +3,8 @@
#include "MapLocationBase.h"
#include "HdLib.h"
#include "MapMatching.hpp"
#include <thread>
#include <mutex>
namespace JfxLocation
{
......@@ -10,6 +12,7 @@ namespace JfxLocation
{
public:
MapLocation(const std::string& project, const std::string& config);
~MapLocation();
virtual int SetMatchingResultCallback(MatchingResultCallback cb);
virtual int PushGPSFrame(GPSFrame& gps);
......@@ -17,8 +20,15 @@ namespace JfxLocation
virtual int PushLocation(Location& loc);
public:
void ThreadProcess();
jf::MapMatching m_mmMatch;
bool m_isRun = false;//记录是否在运行
std::thread m_thread;//运行线程
std::mutex m_mtx;
MatchingResultCallback m_cb = nullptr;
};
}
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment