Skip to content

Commit c3d197f

Browse files
Ramune6110Ramune6110
committed
[add]component
1 parent 5b86011 commit c3d197f

4 files changed

+334
-7
lines changed

‎include/gnss_preprocessing_component.hpp

Lines changed: 73 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
#ifndef __GNSS_PREPROCESSING_COMPONENT_HPP__
2+
#define __GNSS_PREPROCESSING_COMPONENT_HPP__
3+
4+
#include "ros/ros.h"
5+
#include "tf/tf.h"
6+
#include "tf/transform_broadcaster.h"
7+
#include "nav_msgs/Path.h"
8+
#include "nav_msgs/Odometry.h"
9+
#include "geometry_msgs/PoseStamped.h"
10+
#include "geometry_msgs/Pose.h"
11+
#include "sensor_msgs/NavSatFix.h"
12+
13+
#include <iostream>
14+
#include <Eigen/Core>
15+
#include <Eigen/Geometry>
16+
#include <Eigen/Dense>
17+
#include <cmath>
18+
19+
using namespace std;
20+
21+
class GnssPreprocessingComponent
22+
{
23+
public:
24+
GnssPreprocessingComponent(double lat, double lon, double hig);
25+
//GnssPreprocessingComponent(ros::NodeHandle &n, double lat, double lon, double hig);
26+
~GnssPreprocessingComponent();
27+
28+
double pi = 3.1415926535898;
29+
double a = 6378137.0;
30+
double ONE_F = 298.257223563;
31+
double E2 = ((1.0/ONE_F)*(2-(1.0/ONE_F)));
32+
private:
33+
// ros node handle
34+
ros::NodeHandle nh;
35+
36+
// local variable
37+
double lat0;
38+
double lon0;
39+
double hig0;
40+
41+
// publisher
42+
ros::Publisher gnss_pose_pub;
43+
ros::Publisher gnss_path_pub;
44+
45+
// publish data
46+
nav_msgs::Path gnss_path;
47+
48+
// subscriber
49+
ros::Subscriber gnss_sub;
50+
51+
// tf publish
52+
tf::TransformBroadcaster odom_to_baselink_broadcaster;
53+
geometry_msgs::TransformStamped odom_to_baselink_trans;
54+
55+
// callback function
56+
void gnssCallback(const sensor_msgs::NavSatFixConstPtr& gnss_msg);
57+
58+
// convert (lat, lon, alt) to (x, y, z)
59+
double deg2rad(double deg); // Convert degrees to radians
60+
double rad2deg(double rad); // Convert radians to degrees
61+
Eigen::Vector3d blh2ecef(double p_deg, double l_deg, double h);
62+
Eigen::Vector3d ecef2blh(double x, double y, double z);
63+
Eigen::Vector3d ecef2enu(Eigen::Vector3d dest, Eigen::Vector3d origin);
64+
Eigen::Matrix3d rotx(double theta);
65+
Eigen::Matrix3d roty(double theta);
66+
Eigen::Matrix3d rotz(double theta);
67+
68+
// different version convert (lat, lon, alt) to (x, y, z)
69+
int blh2enu(double lat0, double lon0, double lat, double lon, double *x, double *y);
70+
double constrain(double val, double min, double max);
71+
};
72+
73+
#endif // __GNSS_PREPROCESSING_COMPONENT_HPP__

‎include/gnss_preprocessing_core.hpp

Lines changed: 9 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -21,16 +21,19 @@ using namespace std;
2121
class GnssPreprocessingCore
2222
{
2323
public:
24-
GnssPreprocessingCore(ros::NodeHandle &n, double lat, double lon, double hig);
24+
GnssPreprocessingCore(double lat, double lon, double hig);
25+
//GnssPreprocessingCore(ros::NodeHandle &n, double lat, double lon, double hig);
2526
~GnssPreprocessingCore();
2627

2728
double pi = 3.1415926535898;
2829
double a = 6378137.0;
2930
double ONE_F = 298.257223563;
3031
double E2 = ((1.0/ONE_F)*(2-(1.0/ONE_F)));
3132
private:
32-
// local variable
33+
// ros node handle
3334
ros::NodeHandle nh;
35+
36+
// local variable
3437
double lat0;
3538
double lon0;
3639
double hig0;
@@ -70,12 +73,13 @@ class GnssPreprocessingCore
7073
/***********************************************************************
7174
* Initialize
7275
**********************************************************************/
73-
GnssPreprocessingCore::GnssPreprocessingCore(ros::NodeHandle &n, double lat, double lon, double hig)
76+
//GnssPreprocessingCore::GnssPreprocessingCore(ros::NodeHandle &n, double lat, double lon, double hig)
77+
GnssPreprocessingCore::GnssPreprocessingCore(double lat, double lon, double hig)
7478
{
7579
std::cout << "GnssPreprocessingCore Start!" << std::endl;
7680

7781
// init local variable
78-
nh = n;
82+
//nh = n;
7983

8084
lat0 = lat;
8185
lon0 = lon;
@@ -280,7 +284,7 @@ int GnssPreprocessingCore::blh2enu(double lat0, double lon0, double lat, double
280284

281285
*x = static_cast<double>(k * (cos_lat0 * sin_lat - sin_lat0 * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH);
282286
*y = static_cast<double>(k * cos_lat * sin(lon_rad - lon0_rad) * CONSTANTS_RADIUS_OF_EARTH);
283-
287+
284288
return 0;
285289
}
286290

‎src/gnss_preprocessing.cpp

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,18 +1,21 @@
11
#include <ros/ros.h>
22
#include "gnss_preprocessing_core.hpp"
3+
//#include "gnss_preprocessing_component.hpp"
34

45
int main(int argc, char**argv)
56
{
67
ros::init(argc, argv, "gnss_preprocessing");
7-
ros::NodeHandle n;
8+
//ros::NodeHandle n;
89

910
// for utbm_robocar_dataset_20190131_noimage.bag
1011
// Init gps position(latitude, longitude)
1112
double lat0 = 47.5115140833;
1213
double lon0 = 6.79310693333;
1314
double hig0 = 0.0;
1415

15-
GnssPreprocessingCore gnss_preprocessing_core(n, lat0, lon0, hig0);
16+
//GnssPreprocessingCore gnss_preprocessing_core(n, lat0, lon0, hig0);
17+
GnssPreprocessingCore gnss_preprocessing_core(lat0, lon0, hig0);
18+
//GnssPreprocessingComponent gnss_preprocessing_component(lat0, lon0, hig0);
1619

1720
while(ros::ok()){
1821
ros::spinOnce();

‎src/gnss_preprocessing_component.cpp

Lines changed: 247 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,247 @@
1+
#include "ros/ros.h"
2+
#include "tf/tf.h"
3+
#include "tf/transform_broadcaster.h"
4+
#include "nav_msgs/Path.h"
5+
#include "nav_msgs/Odometry.h"
6+
#include "geometry_msgs/PoseStamped.h"
7+
#include "geometry_msgs/Pose.h"
8+
#include "sensor_msgs/NavSatFix.h"
9+
10+
#include <iostream>
11+
#include <Eigen/Core>
12+
#include <Eigen/Geometry>
13+
#include <Eigen/Dense>
14+
#include <cmath>
15+
16+
#include "gnss_preprocessing_component.hpp"
17+
18+
using namespace std;
19+
20+
/***********************************************************************
21+
* Initialize
22+
**********************************************************************/
23+
//GnssPreprocessingComponent::GnssPreprocessingComponent(ros::NodeHandle &n, double lat, double lon, double hig)
24+
GnssPreprocessingComponent::GnssPreprocessingComponent(double lat, double lon, double hig)
25+
{
26+
std::cout << "GnssPreprocessingComponent Start!" << std::endl;
27+
28+
// init local variable
29+
//nh = n;
30+
31+
lat0 = lat;
32+
lon0 = lon;
33+
hig0 = hig;
34+
35+
// Publisher
36+
gnss_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/gnss_pose", 10);
37+
gnss_path_pub = nh.advertise<nav_msgs::Path>("/gnss_path", 10);
38+
39+
// Subscriber
40+
gnss_sub = nh.subscribe("/fix", 10, &GnssPreprocessingComponent::gnssCallback, this);
41+
42+
// init gnss_path
43+
gnss_path.header.frame_id = "map";
44+
gnss_path.header.stamp = ros::Time::now();
45+
gnss_path.header.seq = 0;
46+
}
47+
48+
GnssPreprocessingComponent::~GnssPreprocessingComponent()
49+
{
50+
std::cout << "GnssPreprocessingComponent Finish" << std::endl;
51+
}
52+
53+
void GnssPreprocessingComponent::gnssCallback(const sensor_msgs::NavSatFixConstPtr& gnss_msg)
54+
{
55+
// Convert lat/lon/height to ECEF
56+
/***********************************************************************
57+
NED座標系になっている可能性がある -> enu
58+
enu[0] = N
59+
enu[1] = E
60+
enu[2] = D
61+
となっている可能性があるので、publishする際は現状
62+
X = E = enu[1]
63+
Y = N = enu[0]
64+
としている。
65+
**********************************************************************/
66+
Eigen::Vector3d ecef_origin = blh2ecef(lat0, lon0, hig0);
67+
Eigen::Vector3d ecef = blh2ecef(gnss_msg->latitude, gnss_msg->longitude, gnss_msg->altitude);
68+
Eigen::Vector3d enu = ecef2enu(ecef, ecef_origin);
69+
70+
/***********************************************************************
71+
こちらの処理の場合は正常にENU座標値が算出される
72+
enu[0] = E
73+
enu[1] = N
74+
enu[2] = U
75+
publishする際は
76+
X = E = enu[0]
77+
Y = N = enu[1]
78+
とする。
79+
**********************************************************************/
80+
/*
81+
double x, y;
82+
blh2enu(lat0, lon0, gnss_msg->latitude, gnss_msg->longitude, &x, &y);
83+
Eigen::Vector3d enu = Eigen::Vector3d(x, y, gnss_msg->altitude);
84+
*/
85+
86+
geometry_msgs::PoseStamped point;
87+
point.header.frame_id = "map";
88+
point.header.stamp = ros::Time::now();
89+
point.pose.position.x = enu[1];
90+
point.pose.position.y = enu[0];
91+
point.pose.position.z = 0.0;
92+
point.pose.orientation.w = 0;
93+
point.pose.orientation.x = 0;
94+
point.pose.orientation.y = 0;
95+
point.pose.orientation.z = 1.0;
96+
97+
gnss_path.poses.push_back(point);
98+
gnss_pose_pub.publish(point);
99+
gnss_path_pub.publish(gnss_path);
100+
101+
// /odom to /base_link transform broadcast
102+
odom_to_baselink_trans.header.stamp = ros::Time::now();
103+
odom_to_baselink_trans.header.frame_id = "odom";
104+
odom_to_baselink_trans.child_frame_id = "base_link";
105+
odom_to_baselink_trans.transform.translation.x = enu[1];
106+
odom_to_baselink_trans.transform.translation.y = enu[0];
107+
odom_to_baselink_trans.transform.translation.z = 0.0;
108+
odom_to_baselink_trans.transform.rotation.x = 0.0;
109+
odom_to_baselink_trans.transform.rotation.y = 0.0;
110+
odom_to_baselink_trans.transform.rotation.z = 0.0;
111+
odom_to_baselink_trans.transform.rotation.w = 1.0;
112+
odom_to_baselink_broadcaster.sendTransform(odom_to_baselink_trans);
113+
}
114+
115+
/***********************************************************************
116+
* Convert (lat, lon, alt) -> (x, y, z)
117+
**********************************************************************/
118+
Eigen::Vector3d GnssPreprocessingComponent::blh2ecef(double p_deg, double l_deg, double h) {
119+
double p_rad = deg2rad(p_deg);
120+
double l_rad = deg2rad(l_deg);
121+
122+
double N = (a / sqrt(1.0 - E2 * pow(sin(p_rad), 2)));
123+
124+
double x = (N + h) * cos(p_rad) * cos(l_rad);
125+
double y = (N + h) * cos(p_rad) * sin(l_rad);
126+
double z = (N * (1 - E2) + h) * sin(p_rad);
127+
128+
return Eigen::Vector3d(x, y, z);
129+
}
130+
131+
Eigen::Vector3d GnssPreprocessingComponent::ecef2blh(double x, double y, double z) {
132+
double b = (a * (1.0 - 1.0 / ONE_F));
133+
double ED2 = (E2 * pow(a, 2) / (pow(b, 2)));
134+
double p = sqrt(pow(x, 2) + pow(y, 2));
135+
double si = atan2(z * a, p * b);
136+
137+
double p_rad2 = atan2((z + ED2 * b * pow(sin(si), 3)), (p - E2 * a * pow(cos(si), 3)));
138+
double l_rad2 = atan2(y, x);
139+
140+
double lat = rad2deg(p_rad2);
141+
double lon = rad2deg(l_rad2);
142+
143+
double N = (a / sqrt(1.0 - E2 * pow(sin(p_rad2), 2)));
144+
double hig = (p / cos(p_rad2)) - N;
145+
146+
return Eigen::Vector3d(lat, lon, hig);
147+
}
148+
149+
Eigen::Vector3d GnssPreprocessingComponent::ecef2enu(Eigen::Vector3d dest, Eigen::Vector3d origin) {
150+
Eigen::Vector3d blh = ecef2blh(origin(0), origin(1), origin(2));
151+
152+
Eigen::Matrix3d rotzp1 = rotz(90.0);
153+
Eigen::Matrix3d rotyp = roty(90.0 - blh(0));
154+
Eigen::Matrix3d rotzp2 = rotz(blh(1));
155+
156+
Eigen::Matrix3d mat_conv1 = rotzp1 * rotyp;
157+
Eigen::Matrix3d mat_conv2 = mat_conv1 * rotzp2;
158+
159+
Eigen::Vector3d mov = dest - origin;
160+
161+
return mat_conv2 * mov;
162+
}
163+
164+
Eigen::Matrix3d GnssPreprocessingComponent::rotx(double theta) {
165+
double rad = deg2rad(theta);
166+
167+
Eigen::Matrix3d rotxa;
168+
rotxa << 1, 0, 0,
169+
0, cos(rad), sin(rad),
170+
0, -sin(rad), cos(rad);
171+
return rotxa;
172+
}
173+
174+
Eigen::Matrix3d GnssPreprocessingComponent::roty(double theta) {
175+
double rad = deg2rad(theta);
176+
177+
Eigen::Matrix3d rotya;
178+
rotya << cos(rad), 0, -sin(rad),
179+
0, 1, 0,
180+
sin(rad), 0, cos(rad);
181+
return rotya;
182+
}
183+
184+
Eigen::Matrix3d GnssPreprocessingComponent::rotz(double theta) {
185+
double rad = deg2rad(theta);
186+
187+
Eigen::Matrix3d rotza;
188+
rotza << cos(rad), sin(rad), 0,
189+
-sin(rad), cos(rad), 0,
190+
0, 0, 1;
191+
return rotza;
192+
}
193+
194+
// Convert degrees to radians
195+
double GnssPreprocessingComponent::deg2rad(double deg) {
196+
return deg * pi / 180.0;
197+
}
198+
199+
// Convert radians to degrees
200+
double GnssPreprocessingComponent::rad2deg(double rad) {
201+
return rad * 180.0 / pi;
202+
}
203+
204+
/***********************************************************************
205+
* Convert (lat, lon, alt) -> (x, y, z)
206+
**********************************************************************/
207+
int GnssPreprocessingComponent::blh2enu(double lat0, double lon0, double lat, double lon, double *x, double *y)
208+
{
209+
static constexpr double CONSTANTS_RADIUS_OF_EARTH = 6371000; //[m]
210+
211+
const double lat0_rad = lat0 * (M_PI / 180.0);
212+
const double lon0_rad = lon0 * (M_PI / 180.0);
213+
const double lat_rad = lat * (M_PI / 180.0);
214+
const double lon_rad = lon * (M_PI / 180.0);
215+
216+
const double sin_lat0 = sin(lat0_rad);
217+
const double cos_lat0 = cos(lat0_rad);
218+
const double sin_lat = sin(lat_rad);
219+
const double cos_lat = cos(lat_rad);
220+
221+
const double cos_d_lon = cos(lon_rad - lon0_rad);
222+
223+
const double arg = constrain(sin_lat0 * sin_lat + cos_lat0 * cos_lat * cos_d_lon, -1.0, 1.0);
224+
const double c = acos(arg);
225+
226+
double k = 1.0;
227+
228+
if (fabs(c) > 0) {
229+
k = (c / sin(c));
230+
}
231+
232+
*x = static_cast<double>(k * (cos_lat0 * sin_lat - sin_lat0 * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH);
233+
*y = static_cast<double>(k * cos_lat * sin(lon_rad - lon0_rad) * CONSTANTS_RADIUS_OF_EARTH);
234+
235+
return 0;
236+
}
237+
238+
double GnssPreprocessingComponent::constrain(double val, double min, double max)
239+
{
240+
if (val > max) {
241+
return max;
242+
} else if (val < min) {
243+
return min;
244+
} else {
245+
return val;
246+
}
247+
}

0 commit comments

Comments
 (0)