Skip to content

T
thirdparty_map
Commit 22db92c5 authored by oscar's avatar oscar
Browse files
Options

提交更新

parent 5aa03098
Hide whitespace changes
Inline Side-by-side
Showing with 216 additions and 148 deletions
CMakeLists.txt 0 → 100644
View file @ 22db92c5
cmake_minimum_required(VERSION 3.0.2)
project(test_map)
SET(CMAKE_BUILD_TYPE "Debug")
#SET(CMAKE_BUILD_TYPE "Release")
SET(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -O0 -Wall -g -ggdb")
SET(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall")
include_directories(
${PROJECT_SOURCE_DIR}/
${PROJECT_SOURCE_DIR}/../include/
)
add_library(odrManager SHARED IMPORTED)
set_target_properties(odrManager PROPERTIES IMPORTED_LOCATION ${PROJECT_SOURCE_DIR}/../dynamic/libOdrManager.so)
add_executable(${PROJECT_NAME}
main.cpp
)
target_link_libraries(${PROJECT_NAME}
odrManager
)
\ No newline at end of file
include/MapInterface.hpp
View file @ 22db92c5
#ifndef __JFXMAP_MAPINTERFACE_HPP_
#define __JFXMAP_MAPINTERFACE_HPP_
#include "MapApi.hpp"
#include <string>
#include <vector>
namespace jf {
enum class LaneType {
......@@ -38,6 +39,39 @@ enum class EdgeCrossType {
RightAble = 4, // 向右跨越
};
/**
* @brief 84坐标系中的点
*
*/
struct Point {
Point() : dLon(0), dLat(0), dAlt(0) {};
Point(double inlon, double inlat, double alt);
Point(const Point& o);
Point(const std::vector<double>& d);
Point(const std::map<std::string, double>& m);
/**
* @brief 经度
*
*/
double dLon;
/**
* @brief 维度
*
*/
double dLat;
/**
* @brief 高度
*
*/
double dAlt;
Point& operator=(const Point& o);
bool operator==(const Point& o) const { return dLon == o.dLon && dLat == o.dLat && dAlt == o.dAlt; }
bool operator!=(const Point& o) const { return !(*this == o); }
std::string to_string() const { return std::to_string(dLon) + ", " + std::to_string(dLat) + ", " + std::to_string(dAlt); }
};
class MapInterface {
public:
MapInterface(const std::string &strPrjPath, const std::string strCfgPath);
......@@ -49,8 +83,6 @@ class MapInterface {
bool GetMapData(const Point &ptInLoc, double dCarAngle, long &lOutRaodId, std::vector<long> &vctlOutPreRoadId, std::vector<long> &vctlOutNxtRoadId, int &nLaneCnt, int &nOutLaneNum, LaneType &nOutLaneType, EdgeCrossType &nOutLeftEdgeCrossType,
EdgeCrossType &nOutRightEdgeCrossType, int &nOutSpeedLimit, double &dOutLaneAngle, Point &ptOutFoot);
private:
MapLib m_mlMapLib;
};
} // namespace jf
......
main.cpp 0 → 100644
View file @ 22db92c5

#include <stdio.h>
//#include "OdrManager.h"
#include <vector>
#include "MapInterface.hpp"
using namespace OpenDrive;
int main(int argc, char* argv[])
{
printf("hello world\n");
std::string dir = "";
std::string cfg = "";
jf::MapInterface mapInstance(dir,cfg);
int nLaneCnt = 0/*所在道路的车道总数*/, nOutLaneNum = 0;//当前所在位置的车道序号，从左至右，从1开始
jf::LaneType nOutLaneType = jf::LaneType::NotInvestigated;//车道类型，看头文件枚举
jf::EdgeCrossType nOutLeftEdgeCrossType;//左车道边缘线的跨越属性，具体看头文件枚举
jf::EdgeCrossType nOutRightEdgeCrossType;//右车道边缘线的跨越属性，具体看头文件枚举
int nOutSpeedLimit = 0;//车道最高限速值，单位是千米每小时
double dOutLaneAngle = 0.0;//所在车道，车应该的朝向角度，北为0，顺时针旋转360，0-360
const jf::Point ptInLoc = { objAll.obj.lon, objAll.obj.lat, 0.0 };
jf::Point ptOutFoot;//车道中心线做垂线的点
double dCarAngle = objAll.obj.heading;
long lOutRaodId;
std::vector<long> vctlOutPreRoadId;
std::vector<long> vctlOutNxtRoadId;
//ROS_INFO("begin call jfxmap");
bool result = m_OfflineMap->GetMapData(ptInLoc, dCarAngle, lOutRaodId, vctlOutPreRoadId, vctlOutNxtRoadId,
nLaneCnt, nOutLaneNum, nOutLaneType,
nOutLeftEdgeCrossType, nOutRightEdgeCrossType,
nOutSpeedLimit, dOutLaneAngle, ptOutFoot);
//string filename = "/home/oscar/ros/maps/2022307.xodr";
//string filename = "/home/oscar/ros/maps/dqSample12091.xodr";
//OpenDrive::OdrManager myManager;
//bool bRtn = myManager.LoadFile(filename.c_str());
//printf("return bRtn = %d\n",bRtn);
// const double longitude = 106.474419689;
// const double latitude = 29.67213045;
// std::vector<OdrInfo> odr = myManager.GetOdrInfo(longitude,latitude);
// printf("odr size = %d\n",odr.size());
// for(auto iter: odr)
// {
// printf("roadID = %s,sectionID = %s,laneID = %s, _s = %f. _t = %f \n", iter._roadID,iter._sectionID,iter._laneID,iter._s,iter._t);
// }
//vector<OdrInfo> odr2 = myManager.GetOdrInfoByXY (3926.381879, 11180.870534);
//printf("odr2 size = %d\n",odr2.size());
//for(auto iter: odr2)
//{
// printf("roadID = %s,sectionID = %s,laneID = %s, _s = %f. _t = %f \n", iter._roadID,iter._sectionID,iter._laneID,iter._s,iter._t);
// char** sucId = nullptr;
// int sucSize = 0;
// bool ret0 = myManager.GetSucRoadID(iter._roadID,sucId,sucSize);
// if(ret0)
// {
// printf("GetSucRoadID ret size = %d\n",sucSize);
// for(int i = 0; i < sucSize; i++)
// {
// printf("GetSucRoadID i = %d, roadID = %s\n",i,sucId[i]);
// }
// }
// else
// {
// printf("GetSucRoadID ret = false\n");
// }
// char** preId = nullptr;
// int preSize = 0;
// bool ret1 = myManager.GetPreRoadID(iter._roadID,preId,preSize);
// if(ret1)
// {
// printf("GetPreRoadID ret size = %d\n",preSize);
// for(int i = 0; i < preSize; i++)
// {
// printf("GetPreRoadID i = %d, roadID = %s\n",i,preId[i]);
// }
// }
// else
// {
// printf("GetPreRoadID ret = false\n");
// }
// char** preLId = nullptr;
// int preLSize = 0;
// bool ret2 = myManager.GetPreLaneID(iter._roadID,iter._sectionID,iter._laneID,preLId,preLSize);
// if(ret2)
// {
// printf("GetPreLaneID ret size = %d\n",preLSize);
// for(int i = 0; i < preLSize; i++)
// {
// printf("GetPreLaneID i = %d, laneID = %s\n",i,preLId[i]);
// }
// }
// else
// {
// printf("GetPreLaneID ret = false\n");
// }
// OpenDrive::LaneRelation* vecLaneRelation = nullptr;
// int size = 0;
// bool ret = myManager.GetPreLaneRelationID(iter._roadID,iter._sectionID,iter._laneID,vecLaneRelation,size);
// if(ret)
// {
// printf("LaneRelation ret size = %d\n",size);
// for(int i = 0; i < size; i++)
// {
// printf("LaneRelation i = %d, roadID = %s,sectionID = %s,laneID = %s\n", i,(vecLaneRelation+i)->_roadID,(vecLaneRelation+i)->_sectionID,(vecLaneRelation+i)->_laneID);
// }
// }
// else
// {
// printf("GetPreLaneRelationID ret = false\n");
// }
// char** vecLane = nullptr;
// int vecLSize = 0;
// bool ret3 = myManager.GetAllDrivingLaneIDS(iter._roadID,iter._sectionID,vecLane,vecLSize);
// if(ret3)
// {
// printf("GetAllDrivingLaneIDS ret size = %d\n",vecLSize);
// for(int i = 0; i < vecLSize; i++)
// {
// printf("GetAllDrivingLaneIDS i = %d, laneID = %s\n",i,vecLane[i]);
// }
// }
// else
// {
// printf("GetAllDrivingLaneIDS ret = false\n");
// }
//}
return 0;
}
\ No newline at end of file
src/MapInterface.cpp
View file @ 22db92c5
#include "MapInterface.hpp"
#include <cmath>
#include <iostream>
#include <limits>
namespace jf {
MapInterface::MapInterface(const std::string &strPrjPath, const std::string strCfgPath) { m_mlMapLib.Init(strPrjPath, strCfgPath); }
MapInterface::MapInterface(const std::string& strPrjPath, const std::string strCfgPath)
{
}
MapInterface::~MapInterface() {}
MapInterface::~MapInterface()
{
}
bool MapInterface::GetMapData(const Point &ptInLoc, double dCarAngle, int &nOutLaneCnt, int &nOutLaneNum, LaneType &nOutLaneType, EdgeCrossType &nOutLeftEdgeCrossType, EdgeCrossType &nOutRightEdgeCrossType, int &nOutSpeedLimit, double &dOutLaneAngle,
Point &ptOutFoot) {
Point &ptOutFoot)
{
long lRoadId;
std::vector<long> vctlPreRoadId = {};
std::vector<long> vctlNxtRoadId = {};
......@@ -18,145 +22,8 @@ bool MapInterface::GetMapData(const Point &ptInLoc, double dCarAngle, int &nOutL
}
bool MapInterface::GetMapData(const Point &ptInLoc, double dCarAngle, long &lOutRaodId, std::vector<long> &vctlOutPreRoadId, std::vector<long> &vctlOutNxtRoadId, int &nOutLaneCnt, int &nOutLaneNum, LaneType &nOutLaneType,
EdgeCrossType &nOutLeftEdgeCrossType, EdgeCrossType &nOutRightEdgeCrossType, int &nOutSpeedLimit, double &dOutLaneAngle, Point &ptOutFoot) {
std::vector<HdLinkId> vcthlLinkIds = {};
std::vector<HdLaneId> vcthlLaneIds = {};
// Locate lane failed
if (false == m_mlMapLib.GetLocateLanes(ptInLoc, vcthlLinkIds, vcthlLaneIds)) {
nOutLaneCnt = 0;
nOutLaneNum = 0;
nOutLaneType = LaneType::NotInvestigated;
nOutLeftEdgeCrossType = EdgeCrossType::NotInvestigated;
nOutRightEdgeCrossType = EdgeCrossType::NotInvestigated;
nOutSpeedLimit = 0;
dOutLaneAngle = 0.0;
ptOutFoot = ptInLoc;
//std::cout << "MapInterface::GetMapData(), Locate lane failed." << std::endl;
return false;
}
int nIndex = 0;
double dMinAngleDiff = std::numeric_limits<double>::max();
auto func_GetAngleDiff = [](double dAngle1, double dAngle2) { return std::min(std::abs(dAngle1 - dAngle2), 360 - std::abs((dAngle1 - dAngle2))); };
for (int i = 0; i < vcthlLinkIds.size(); i++) {
const HdLinkId &hlLinkId = vcthlLinkIds[i];
const HdLaneId &hlLaneId = vcthlLaneIds[i];
double dAzimuth = 0;
if (vcthlLinkIds.size() == 1) {
nIndex = i;
} else {
if (true == m_mlMapLib.GetLaneAzimuth(hlLinkId, hlLaneId, ptInLoc, dAzimuth) || true == m_mlMapLib.GetLinkStartAngle(hlLinkId, dAzimuth)) {
double dAngleDiff = func_GetAngleDiff(dCarAngle, dAzimuth);
if (dMinAngleDiff >= dAngleDiff) {
dMinAngleDiff = dAngleDiff;
nIndex = i;
}
}
}
}
lOutRaodId = vcthlLinkIds[nIndex].lValue;
std::vector<HdLinkId> vctliPLinkIds = {};
std::vector<HdLinkId> vctliNLinkIds = {};
m_mlMapLib.GetLinkPreNxt(vcthlLinkIds[nIndex], vctliPLinkIds, vctliNLinkIds);
for (int i = 0; i < vctliPLinkIds.size(); ++i) {
vctlOutPreRoadId.push_back(vctliPLinkIds.at(i).lValue);
}
for (int i = 0; i < vctliNLinkIds.size(); ++i) {
vctlOutNxtRoadId.push_back(vctliNLinkIds.at(i).lValue);
}
if (false == m_mlMapLib.GetLaneCnt(vcthlLinkIds[nIndex], nOutLaneCnt)) {
nOutLaneCnt = 0;
nOutLaneNum = 0;
nOutLaneType = LaneType::NotInvestigated;
nOutLeftEdgeCrossType = EdgeCrossType::NotInvestigated;
nOutRightEdgeCrossType = EdgeCrossType::NotInvestigated;
nOutSpeedLimit = 0;
dOutLaneAngle = 0.0;
ptOutFoot = {};
std::cout << "MapInterface::GetMapData(), get lane count failed." << std::endl;
return false;
}
if (false == m_mlMapLib.GetLaneNum(vcthlLinkIds[nIndex], vcthlLaneIds[nIndex], nOutLaneNum)) {
nOutLaneNum = 0;
nOutLaneType = LaneType::NotInvestigated;
nOutLeftEdgeCrossType = EdgeCrossType::NotInvestigated;
nOutRightEdgeCrossType = EdgeCrossType::NotInvestigated;
nOutSpeedLimit = 0;
dOutLaneAngle = 0.0;
ptOutFoot = {};
std::cout << "MapInterface::GetMapData(), get lane num failed." << std::endl;
return false;
}
if (false == m_mlMapLib.GetLaneType(vcthlLinkIds[nIndex], nOutLaneNum, (int &)nOutLaneType)) {
nOutLaneType = LaneType::NotInvestigated;
nOutLeftEdgeCrossType = EdgeCrossType::NotInvestigated;
nOutRightEdgeCrossType = EdgeCrossType::NotInvestigated;
nOutSpeedLimit = 0;
dOutLaneAngle = 0.0;
ptOutFoot = {};
std::cout << "MapInterface::GetMapData(), get lane type failed." << std::endl;
return false;
}
if (false == m_mlMapLib.GetLaneEdgeCrossType(vcthlLinkIds[nIndex], nOutLaneNum, true, (int &)nOutLeftEdgeCrossType)) {
nOutLeftEdgeCrossType = EdgeCrossType::NotInvestigated;
nOutRightEdgeCrossType = EdgeCrossType::NotInvestigated;
nOutSpeedLimit = 0;
dOutLaneAngle = 0.0;
ptOutFoot = {};
std::cout << "MapInterface::GetMapData(), get left lane edge cross failed." << std::endl;
return false;
}
if (false == m_mlMapLib.GetLaneEdgeCrossType(vcthlLinkIds[nIndex], nOutLaneNum, false, (int &)nOutRightEdgeCrossType)) {
nOutRightEdgeCrossType = EdgeCrossType::NotInvestigated;
nOutSpeedLimit = 0;
dOutLaneAngle = 0.0;
ptOutFoot = {};
std::cout << "MapInterface::GetMapData(), get right lane cross failed." << std::endl;
return false;
}
if (false == m_mlMapLib.GetLinkSpeedLimit(vcthlLinkIds[nIndex], nOutSpeedLimit)) {
nOutSpeedLimit = 0;
dOutLaneAngle = 0.0;
ptOutFoot = {};
std::cout << "MapInterface::GetMapData(), get link speed failed." << std::endl;
return false;
}
if (false == m_mlMapLib.GetLaneAzimuth(vcthlLinkIds[nIndex], vcthlLaneIds[nIndex], ptInLoc, dOutLaneAngle)) {
dOutLaneAngle = 0.0;
ptOutFoot = {};
std::cout << "MapInterface::GetMapData(), get lane angle failed." << std::endl;
return false;
}
int nSeqNum = 0;
if (false == m_mlMapLib.GetLaneNum(vcthlLinkIds[nIndex], vcthlLaneIds[nIndex], nSeqNum)) {
ptOutFoot = {};
std::cout << "MapInterface::GetMapData(), get lane num failed." << std::endl;
return false;
} else {
std::vector<std::vector<double>> vctvdGeoLink = {};
double dDistance = 0;
if (false == m_mlMapLib.GetLaneGeoLink(vcthlLinkIds[nIndex], nSeqNum, vctvdGeoLink)) {
ptOutFoot = {};
//std::cout << "MapInterface::GetMapData(), get lane geolink failed." << std::endl;
return false;
} else {
if (false == m_mlMapLib.GetPerpendicularFoot(ptInLoc, vctvdGeoLink, ptOutFoot, dDistance)) {
ptOutFoot = {};
std::cout << "MapInterface::GetMapData(), get lane foot point failed." << std::endl;
return false;
}
};
}
EdgeCrossType &nOutLeftEdgeCrossType, EdgeCrossType &nOutRightEdgeCrossType, int &nOutSpeedLimit, double &dOutLaneAngle, Point &ptOutFoot)
{
return true;
}
......
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