1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
#ifndef _ROS_MATCH_H_
#define _ROS_MATCH_H_
#include <Gaussian.h>
#include <ros/ros.h>
#include "CoordSys.h"
#include "MapMatching.hpp"
#include "adu_msgs/MeData.h"
#include "can_msgs/Frame.h"
#include "gnss_imu_msgs/GNSS.h"
#include "jc_map_msgs/MatchingResult.h"
#include "localization_msgs/Localization.h"
namespace jf {
class RosMatch {
public:
RosMatch();
~RosMatch();
// 接受GNSS的信息
void ReceiveGnss(const gnss_imu_msgs::GNSS::ConstPtr& msg);
// 接受EyeQ4的信息
void ReceiveAduData(const adu_msgs::MeData::ConstPtr& msg);
// 接受egomotion的信息
void ReceiveLocalization(const localization_msgs::Localization::ConstPtr& msg);
private:
// 发布地图匹配结果的进程
void PublishMatchResultProc();
// 发布地图输出数据的进程
void PublishMapInfoProc();
// 发布地图匹配结果
void PublishMatchResult(const IGnssFootLocation& data);
// 发布定位信息的CanFd消息
void PublishLocCanFd(const localization_msgs::Localization::ConstPtr& msg, double speed);
// 转换车道位置信息
bool ConvertLanePos(int src, LaneMarkingSide& dst);
// 转换车道颜色
bool ConvertLaneColor(int src, HMC::LaneMarkingColor& dst);
// 转换车道跨越属性
bool ConvertLaneCross(int src_double, int src_single, HMC::LaneMarkingType& dst);
// 转换道路边界线位置
bool ConvertLinkPos(int src, LinkMarkingSide& dst);
// 转换标牌
bool ConvertTsr(const adu_msgs::TsrInfo& src, ITrafsignMap& dst);
private:
// 地图匹配相关
MapMatching m_mmMatch;
ros::NodeHandle m_nhNodeHandle;
ros::Subscriber m_subGnss;
ros::Subscriber m_subEyeQ4;
ros::Subscriber m_subLoc;
ros::Publisher m_pubLocCanFd;
ros::Publisher m_pubMatchResult;
std::thread* m_thdPubMatchResult;
std::thread* pub_map_info_thread_;
};
} // namespace jf
#endif // _ROS_MATCH_H_