Commit 52a9d1d3 authored by oscar's avatar oscar

提交修改代码

parent 068af276
Pipeline #876 canceled with stages
cmake_minimum_required(VERSION 2.8.3)
project(jfxmap_python)
option(TARGET_CORE_LIB "Core lib" ON)
option(TARGET_MAP_PRE_EXE "map pre exe" OFF)
option(TARGET_MAPPER_LIB "mapper lib" OFF)
option(TARGET_MAPPER_EXE "mapper exe" OFF)
set(CMAKE_DEBUG_POSTFIX _d)
if("${BUILD_PLATFORM}" STREQUAL "")
set(BUILD_PLATFORM "BUILD_ON_LINUX" CACHE STRING "Build on target platform")
endif()
message(STATUS "============= ${PROJECT_NAME} : ${BUILD_PLATFORM} ===========")
if("${BUILD_PLATFORM}" STREQUAL "BUILD_ON_LINUX")
set(CMAKE_C_STANDARD 99)
set(CMAKE_CXX_STANDARD 11)
endif()
set(PYBIND11_CPP_STANDARD -std=c++11)
find_package(pybind11 REQUIRED)
INCLUDE_DIRECTORIES(
${pybind11_INCLUDE_DIRS}
./
)
if(TARGET_CORE_LIB )
file(GLOB SRC_LIST
"../offline/Core/*.cpp"
"../submodules/xxtea-c/*.c"
)
add_library(jfxmap_python SHARED ${SRC_LIST} MapInterface.cpp jfxmap_python.cpp)
set_target_properties(jfxmap_python PROPERTIES OUTPUT_NAME "jfxmap_python")
if("${BUILD_PLATFORM}" STREQUAL "BUILD_ON_ANDROID")
target_link_libraries(jfxmap_python pybind11::module log)
else()
target_link_libraries(jfxmap_python pybind11::module pthread)
endif()
set(local_include_directories
${local_include_directories}
${PROJECT_SOURCE_DIR}/../offline/Core
${PROJECT_SOURCE_DIR}/../offline/Public
${PROJECT_SOURCE_DIR}/../submodules/ajson
${PROJECT_SOURCE_DIR}/../submodules/spdlog/include
)
endif()
if(TARGET_MAP_PRE_EXE)
# file(GLOB SRC_LIST
# "offline/MapPre/*.cpp"
# "submodules/tinyxml/src/*.cpp"
# )
# add_executable(map_pre_exe ${SRC_LIST})
# # add_dependencies(map_pre_exe core_lib_dynamic)
# add_definitions(-DSTANDALONE)
# link_directories(${PROJECT_SOURCE_DIR}/build)
# link_directories(${PROJECT_SOURCE_DIR}/submodules/shapelib/build)
# link_directories(/usr/local/lib)
# target_link_libraries(map_pre_exe core_lib_dynamic)
# target_link_libraries(map_pre_exe shp)
# set(local_include_directories
# ${local_include_directories}
# ${PROJECT_SOURCE_DIR}/offline/core
# ${PROJECT_SOURCE_DIR}/offline/mapPre
# ${PROJECT_SOURCE_DIR}/submodules/shapelib
# )
endif()
if(TARGET_UNIT_TEST_EXE)
# file(GLOB SRC_LIST
# "offline/Core/*.cpp"
# "offline/Projects/qichecheng/*.cpp"
# "offline/UnitTest/*.cpp"
# "submodules/xxtea-c/*.c"
# )
# add_executable(unit_test_exe ${SRC_LIST})
# add_definitions(-DSTANDALONE)
# add_dependencies(unit_test_exe core_lib_dynamic)
# link_directories(${PROJECT_SOURCE_DIR}/build/devel/lib)
# message(STATUS "dir = ${PROJECT_SOURCE_DIR}/build/devel/lib")
# link_directories(/usr/local/lib)
# # target_link_libraries(unit_test_exe core_lib_dynamic)
# # target_link_libraries(unit_test_exe ${PROJECT_SOURCE_DIR}/build/devel/lib/libcore_lib_d.so)
# target_link_libraries(unit_test_exe pthread)
# set(local_include_directories
# ${local_include_directories}
# ${PROJECT_SOURCE_DIR}/offline/Core
# ${PROJECT_SOURCE_DIR}/offline/UnitTest
# ${PROJECT_SOURCE_DIR}/offline/UnitTest/fusion_localization/include
# ${PROJECT_SOURCE_DIR}/offline/UnitTest/hd_map/hd_map_common/include
# ${PROJECT_SOURCE_DIR}/offline/UnitTest/utils/common/include
# ${PROJECT_SOURCE_DIR}/offline/Projects
# )
endif()
set(local_include_directories
${local_include_directories}
${PROJECT_SOURCE_DIR}/../submodules/ajson
${PROJECT_SOURCE_DIR}/../submodules/tinyxml/include
${PROJECT_SOURCE_DIR}/../submodules/boolinq/include/boolinq
${PROJECT_SOURCE_DIR}/../submodules/jfxthread/include
${PROJECT_SOURCE_DIR}/../submodules/queue-thread-safe
${PROJECT_SOURCE_DIR}/../submodules/spdlog/include
${PROJECT_SOURCE_DIR}/../submodules/xxtea-c
)
include_directories(${local_include_directories})
get_directory_property(hasParent PARENT_DIRECTORY)
if(hasParent)
set(jfxoffline_include_directories ${local_include_directories} PARENT_SCOPE)
endif()
#include "MapInterface.hpp"
#include <cmath>
#include <iostream>
#include <limits>
namespace jf {
MapInterface::MapInterface() { }
MapInterface::~MapInterface() {}
void MapInterface::Init(const std::string& strPrjPath, const std::string strCfgPath)
{
m_mlMapLib.Init(strPrjPath, strCfgPath);
}
bool MapInterface::GetMapData(const Point &ptInLoc, double dCarAngle, int &nOutLaneCnt, int &nOutLaneNum, LaneType &nOutLaneType, EdgeCrossType &nOutLeftEdgeCrossType, EdgeCrossType &nOutRightEdgeCrossType, int &nOutSpeedLimit, double &dOutLaneAngle,
Point &ptOutFoot) {
long lRoadId;
std::vector<long> vctlPreRoadId = {};
std::vector<long> vctlNxtRoadId = {};
return GetMapData(ptInLoc, dCarAngle, lRoadId, vctlPreRoadId, vctlNxtRoadId, nOutLaneCnt, nOutLaneNum, nOutLaneType, nOutLeftEdgeCrossType, nOutRightEdgeCrossType, nOutSpeedLimit, dOutLaneAngle, ptOutFoot);
}
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;
}
};
}
return true;
}
} // namespace jf
\ No newline at end of file
#ifndef __JFXMAP_MAPINTERFACE_HPP_
#define __JFXMAP_MAPINTERFACE_HPP_
#include "MapApi.hpp"
namespace jf {
enum class LaneType {
NotInvestigated = 0, // 未调查
NormalLane = 1, // 普通车道
EnterLane = 2, // 入口车道
ExitLane = 3, // 出口车道
ConnectLane = 4, // 连接车道
EmergencyLane = 5, // 应急车道
StopLane = 6, // 停车道
EmergencyStopLane = 7, // 紧急停车道
AcceleratingLane = 8, // 加速车道
DeceleratingLane = 9, // 减速车道
HazardLane = 10, // 避险车道
JunctionLane = 11, // 路口车道
TollStationLane = 12, // 收费站车道
CheckpointLane = 13, // 检查站车道
UTurnLane = 14, // 掉头车道
BusLane = 15, // 公交车道
TidalLane = 16, // 潮汐(可变) 车道
LocomotiveLane = 17, // 机车车道
LocomotiveParkingArea = 18, // 机车停车区
HOVHighLoadLane = 19, // HOV 高承载车道
NonMotorizedLane = 30, // 非机动车道
Sidewalk = 40, // 人行道
Ohters = 99, // 其它车道
};
enum class EdgeCrossType {
NotInvestigated = 0, // 未调查
Unable = 1, // 无法跨越
BidirecAble = 2, // 双方向跨越
LeftAble = 3, // 向左跨越
RightAble = 4, // 向右跨越
};
class MapInterface {
public:
MapInterface();
~MapInterface();
public:
void Init(const std::string& strPrjPath, const std::string strCfgPath);
bool GetMapData(const Point &ptInLoc, double dCarAngle, int &nOutLaneCnt, int &nOutLaneNum, LaneType &nOutLaneType, EdgeCrossType &nOutLeftEdgeCrossType, EdgeCrossType &nOutRightEdgeCrossType, int &nOutSpeedLimit, double &dOutLaneAngle,
Point &ptOutFoot);
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
#endif
\ No newline at end of file
import os
import sys
sys.path.append("./build/")
import libjfxmap_python
ret = libjfxmap_python.add(3,4)
print(ret)
p_dir = "/home/oscar/ros/git/catkin_ws_xishan/src/jfx_traffic_events/thirdparty/jfxmap/"
f_path = "./maps/xishan/mapconfig.json"
is_ok = libjfxmap_python.InitMap(p_dir,f_path)
print(is_ok)
info = libjfxmap_python.GetMapData(31.5867973272,120.4137985087,262);
print(info.isInMap);
print(info.lOutRaodId);
print(info.nLaneCnt);
print(info.nOutLaneNum);
print(info.nOutLaneType);
print(info.dOutLaneAngle);
print(info.ptOutFoot_lat);
print(info.vctlOutPreRoadId);
print(info.vctlOutNxtRoadId);
# build module for 3.6
mkdir -p build-python-3.6
(cd build-python-3.6 ; cmake .. -DPYBIND11_PYTHON_VERSION=3.6 ; make )
# build module for 2.7
mkdir -p build-python-2.7
(cd build-python-2.7 ; cmake .. -DPYBIND11_PYTHON_VERSION=2.7 ; make )
\ No newline at end of file
#include "jfxmap_python.h"
#include "MapInterface.hpp"
#include <memory>
jf::MapInterface g_jfMap;
static int g_isInit = 0;
MapInfo g_info;
int add(int i, int j)
{
return i + j;
}
bool InitMap(std::string strPrjPath, std::string strCfgPath)
{
printf("strPrjPath = %s, strCfgPath = %s g_isInit = %d", strPrjPath.c_str(), strCfgPath.c_str(), g_isInit);
//if (g_jfMap == nullptr)
//{
// g_jfMap = new jf::MapInterface();
// //g_jfMap->Init(strPrjPath, strCfgPath);
// return true;
//}
if (g_isInit == 0)
{
g_jfMap.Init(strPrjPath, strCfgPath);
g_isInit = 1;
return true;
}
return false;
}
MapInfo* GetMapData(double lat, double lon, double carAngle)
{
memset(&g_info, 0, sizeof(g_info));
if (g_isInit == 1)
{
const jf::Point ptInLoc = { lon, lat, 0.0 };
long lOutRaodId;
std::vector<long> vctlOutPreRoadId;
std::vector<long> vctlOutNxtRoadId;
int nLaneCnt;
int nOutLaneNum;
jf::LaneType nOutLaneType;
jf::EdgeCrossType nOutLeftEdgeCrossType;
jf::EdgeCrossType nOutRightEdgeCrossType;
int nOutSpeedLimit;
double dOutLaneAngle;
jf::Point ptOutFoot;
if (g_jfMap.GetMapData(ptInLoc, carAngle, lOutRaodId, vctlOutPreRoadId, vctlOutNxtRoadId, nLaneCnt, nOutLaneNum, nOutLaneType, nOutLeftEdgeCrossType, nOutRightEdgeCrossType, nOutSpeedLimit, dOutLaneAngle, ptOutFoot))
{
g_info.isInMap = 1;
g_info.lOutRaodId = lOutRaodId;
g_info.vctlOutPreRoadId = vctlOutPreRoadId;
g_info.vctlOutNxtRoadId = vctlOutNxtRoadId;
g_info.nLaneCnt = nLaneCnt;
g_info.nOutLaneNum = nOutLaneNum;
g_info.nOutLaneType = static_cast<int>(nOutLaneType);
g_info.nOutLeftEdgeCrossType = static_cast<int>(nOutLeftEdgeCrossType);
g_info.nOutRightEdgeCrossType = static_cast<int>(nOutRightEdgeCrossType);
g_info.nOutSpeedLimit = nOutSpeedLimit;
g_info.dOutLaneAngle = dOutLaneAngle;
g_info.ptOutFoot_lat = ptOutFoot.dLat;
g_info.ptOutFoot_lon = ptOutFoot.dLon;
}
}
return &g_info;
}
#ifndef _JFXMAP_PYTHON_H_
#define _JFXMAP_PYTHON_H_
#include <pybind11/pybind11.h>
#include <pybind11/stl_bind.h>
#include <pybind11/stl.h>
#include <string>
namespace py = pybind11;
struct MapInfo
{
int isInMap;
long lOutRaodId;
std::vector<long> vctlOutPreRoadId;
std::vector<long> vctlOutNxtRoadId;
int nLaneCnt;
int nOutLaneNum;
int nOutLaneType;
int nOutLeftEdgeCrossType;
int nOutRightEdgeCrossType;
int nOutSpeedLimit;
double dOutLaneAngle;
double ptOutFoot_lat;
double ptOutFoot_lon;
};
int add(int i, int j);
bool InitMap(std::string strPrjPath, std::string strCfgPath);
MapInfo* GetMapData(double lat,double lon,double carAngle);
PYBIND11_MODULE(libjfxmap_python, m)
{
// optional module docstring
m.doc() = "pybind11 example plugin";
py::class_<MapInfo>(m, "MapInfo")
.def_readwrite("isInMap", &MapInfo::isInMap)
.def_readwrite("lOutRaodId", &MapInfo::lOutRaodId)
.def_readwrite("vctlOutPreRoadId", &MapInfo::vctlOutPreRoadId)
.def_readwrite("vctlOutNxtRoadId", &MapInfo::vctlOutNxtRoadId)
.def_readwrite("nLaneCnt", &MapInfo::nLaneCnt)
.def_readwrite("nOutLaneNum", &MapInfo::nOutLaneNum)
.def_readwrite("nOutLaneType", &MapInfo::nOutLaneType)
.def_readwrite("nOutLeftEdgeCrossType", &MapInfo::nOutLeftEdgeCrossType)
.def_readwrite("nOutRightEdgeCrossType", &MapInfo::nOutRightEdgeCrossType)
.def_readwrite("nOutSpeedLimit", &MapInfo::nOutSpeedLimit)
.def_readwrite("dOutLaneAngle", &MapInfo::dOutLaneAngle)
.def_readwrite("ptOutFoot_lat", &MapInfo::ptOutFoot_lat)
.def_readwrite("ptOutFoot_lon", &MapInfo::ptOutFoot_lon);
// expose add function, and add keyword arguments and default arguments
m.def("add", &add, "A function which adds two numbers", py::arg("i") = 1, py::arg("j") = 2);
m.def("InitMap", &InitMap, "A function which init map", py::return_value_policy::copy);
m.def("GetMapData", &GetMapData, "A function which GetInfo", py::return_value_policy::reference);
// exporting variables
m.attr("the_answer") = 42;
py::object world = py::cast("World");
m.attr("what") = world;
}
#endif
\ No newline at end of file
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