Skip to content

Commit a051f72

Browse files
Ramune6110Ramune6110
committed
[add]include src
1 parent d2884d9 commit a051f72

6 files changed

+452
-0
lines changed

‎CMakeLists.txt

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,3 +26,6 @@ include_directories(
2626
${catkin_INCLUDE_DIRS}
2727
${EIGEN3_INCLUDE_DIRS}
2828
)
29+
30+
add_executable(gnss_preprocessing src/gnss_preprocessing.cpp)
31+
target_link_libraries(gnss_preprocessing ${catkin_LIBRARIES})

‎include/gnss_preprocessing_component.hpp

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
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+
class GnssPreprocessingComponent
20+
{
21+
public:
22+
//GnssPreprocessingComponent();
23+
GnssPreprocessingComponent(ros::NodeHandle &n);
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::NodeHandle nh;
34+
35+
// gps callback
36+
void gnssCallback(const sensor_msgs::NavSatFixConstPtr& gnss_msg);
37+
38+
double deg2rad(double deg); // Convert degrees to radians
39+
double rad2deg(double rad); // Convert radians to degrees
40+
Eigen::Vector3d blh2ecef(double p_deg, double l_deg, double h);
41+
Eigen::Vector3d ecef2blh(double x, double y, double z);
42+
Eigen::Vector3d ecef2enu(Eigen::Vector3d dest, Eigen::Vector3d origin);
43+
Eigen::Matrix3d rotx(double theta);
44+
Eigen::Matrix3d roty(double theta);
45+
Eigen::Matrix3d rotz(double theta);
46+
};
47+
48+
#endif // __GNSS_PREPROCESSING_COMPONENT_HPP__

‎include/gnss_preprocessing_core.hpp

Lines changed: 207 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,207 @@
1+
#ifndef __GNSS_PREPROCESSING_CORE_HPP__
2+
#define __GNSS_PREPROCESSING_CORE_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 GnssPreprocessingCore
22+
{
23+
public:
24+
//GnssPreprocessingCore();
25+
GnssPreprocessingCore(ros::NodeHandle &n);
26+
//GnssPreprocessingCore(double lat, double lon, double hig);
27+
//GnssPreprocessingCore(ros::NodeHandle &n, double lat, double lon, double hig);
28+
~GnssPreprocessingCore();
29+
30+
double pi = 3.1415926535898;
31+
double a = 6378137.0;
32+
double ONE_F = 298.257223563;
33+
double E2 = ((1.0/ONE_F)*(2-(1.0/ONE_F)));
34+
private:
35+
ros::NodeHandle nh;
36+
37+
// publisher
38+
39+
// publish data
40+
nav_msgs::Path gnss_path;
41+
42+
// subscriber
43+
ros::Subscriber gnss_sub;
44+
45+
// callback function
46+
void gnssCallback(const sensor_msgs::NavSatFixConstPtr& gnss_msg);
47+
48+
double deg2rad(double deg); // Convert degrees to radians
49+
double rad2deg(double rad); // Convert radians to degrees
50+
Eigen::Vector3d blh2ecef(double p_deg, double l_deg, double h);
51+
Eigen::Vector3d ecef2blh(double x, double y, double z);
52+
Eigen::Vector3d ecef2enu(Eigen::Vector3d dest, Eigen::Vector3d origin);
53+
Eigen::Matrix3d rotx(double theta);
54+
Eigen::Matrix3d roty(double theta);
55+
Eigen::Matrix3d rotz(double theta);
56+
};
57+
58+
/***********************************************************************
59+
* Initialize
60+
**********************************************************************/
61+
GnssPreprocessingCore::GnssPreprocessingCore(ros::NodeHandle &n)
62+
{
63+
std::cout << "GnssPreprocessingCore Start!" << std::endl;
64+
65+
// ROS Node
66+
nh = n;
67+
68+
// Publisher
69+
//gnss_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/gnss_pose", 10);
70+
//gnss_path_pub = nh.advertise<nav_msgs::Path>("/gnss_path", 10);
71+
72+
// Subscriber
73+
gnss_sub = nh.subscribe("/fix", 10, &GnssPreprocessingCore::gnssCallback, this);
74+
75+
// init gnss_path
76+
gnss_path.header.frame_id = "map";
77+
gnss_path.header.stamp = ros::Time::now();
78+
gnss_path.header.seq = 0;
79+
}
80+
81+
GnssPreprocessingCore::~GnssPreprocessingCore()
82+
{
83+
std::cout << "GnssPreprocessingCore Finish" << std::endl;
84+
}
85+
86+
Eigen::Vector3d GnssPreprocessingCore::blh2ecef(double p_deg, double l_deg, double h) {
87+
double p_rad = deg2rad(p_deg);
88+
double l_rad = deg2rad(l_deg);
89+
90+
double NN = (a / sqrt(1.0 - (E2)*pow(sin(p_rad),2)));
91+
92+
double x = (NN+h)*cos(p_rad)*cos(l_rad);
93+
double y = (NN+h)*cos(p_rad)*sin(l_rad);
94+
double z = (NN*(1-E2)+h)*sin(p_rad);
95+
96+
return Eigen::Vector3d(x, y, z);
97+
}
98+
99+
void GnssPreprocessingCore::gnssCallback(const sensor_msgs::NavSatFixConstPtr& gnss_msg)
100+
{
101+
std::cout << "gnss callback" << std::endl;
102+
103+
/*
104+
data_conversion_gps(gps_msg, gps_data);
105+
106+
geometry_msgs::PoseStamped point;
107+
point.header.frame_id = "map";
108+
point.header.stamp = ros::Time::now();
109+
point.pose.position.x = gps_data.ned[0];
110+
point.pose.position.y = gps_data.ned[1];
111+
point.pose.position.z = 0.0;
112+
point.pose.orientation.w = 0;
113+
point.pose.orientation.x = 0;
114+
point.pose.orientation.y = 0;
115+
point.pose.orientation.z = 1.0;
116+
gps_path.poses.push_back(point);
117+
gps_pose_pub.publish(point);
118+
gps_path_pub.publish(gps_path);
119+
120+
// /odom to /base_link transform broadcast
121+
odom_to_baselink_trans.header.stamp = ros::Time::now();
122+
odom_to_baselink_trans.header.frame_id = "odom";
123+
odom_to_baselink_trans.child_frame_id = "base_link";
124+
odom_to_baselink_trans.transform.translation.x = gps_data.ned[0];
125+
odom_to_baselink_trans.transform.translation.y = gps_data.ned[1];
126+
odom_to_baselink_trans.transform.translation.z = 0.0;
127+
odom_to_baselink_trans.transform.rotation.x = 0.0;
128+
odom_to_baselink_trans.transform.rotation.y = 0.0;
129+
odom_to_baselink_trans.transform.rotation.z = 0.0;
130+
odom_to_baselink_trans.transform.rotation.w = 1.0;
131+
odom_to_baselink_broadcaster.sendTransform(odom_to_baselink_trans);
132+
*/
133+
}
134+
135+
Eigen::Vector3d GnssPreprocessingCore::ecef2blh(double x, double y, double z) {
136+
double b = (a*(1.0 - 1.0/ONE_F));
137+
double ED2 = (E2*pow(a,2)/(pow(b,2)));
138+
double p = sqrt(pow(x,2) + pow(y,2));
139+
double si = atan2(z*a, p*b);
140+
141+
double p_rad2 = atan2((z + ED2*b*pow(sin(si),3)),(p-E2*a*pow(cos(si),3)));
142+
double l_rad2 = atan2(y, x);
143+
144+
double lat = rad2deg(p_rad2);
145+
double lon = rad2deg(l_rad2);
146+
147+
double NN = (a / sqrt(1.0 - (E2)*pow(sin(p_rad2),2)));
148+
double hig = (p/cos(p_rad2)) - NN;
149+
150+
return Eigen::Vector3d(lat, lon, hig);
151+
}
152+
153+
Eigen::Vector3d GnssPreprocessingCore::ecef2enu(Eigen::Vector3d dest, Eigen::Vector3d origin) {
154+
Eigen::Vector3d blh = ecef2blh(origin(0), origin(1), origin(2));
155+
156+
Eigen::Matrix3d rotzp1 = rotz(90.0);
157+
Eigen::Matrix3d rotyp = roty(90.0-blh(0));
158+
Eigen::Matrix3d rotzp2 = rotz(blh(1));
159+
160+
Eigen::Matrix3d mat_conv1 = rotzp1*rotyp;
161+
Eigen::Matrix3d mat_conv2 = mat_conv1*rotzp2;
162+
163+
Eigen::Vector3d mov = dest - origin;
164+
return mat_conv2*mov;
165+
}
166+
167+
Eigen::Matrix3d GnssPreprocessingCore::rotx(double theta) {
168+
double rad = deg2rad(theta);
169+
170+
Eigen::Matrix3d rotxa;
171+
rotxa << 1, 0, 0,
172+
0, cos(rad), sin(rad),
173+
0, -sin(rad), cos(rad);
174+
return rotxa;
175+
}
176+
177+
Eigen::Matrix3d GnssPreprocessingCore::roty(double theta) {
178+
double rad = deg2rad(theta);
179+
180+
Eigen::Matrix3d rotya;
181+
rotya << cos(rad), 0, -sin(rad),
182+
0, 1, 0,
183+
sin(rad), 0, cos(rad);
184+
return rotya;
185+
}
186+
187+
Eigen::Matrix3d GnssPreprocessingCore::rotz(double theta) {
188+
double rad = deg2rad(theta);
189+
190+
Eigen::Matrix3d rotza;
191+
rotza << cos(rad), sin(rad), 0,
192+
-sin(rad), cos(rad), 0,
193+
0, 0, 1;
194+
return rotza;
195+
}
196+
197+
// Convert degrees to radians
198+
double GnssPreprocessingCore::deg2rad(double deg) {
199+
return deg * pi / 180.0;
200+
}
201+
202+
// Convert radians to degrees
203+
double GnssPreprocessingCore::rad2deg(double rad) {
204+
return rad * 180.0 / pi;
205+
}
206+
207+
#endif // __GNSS_PREPROCESSING_CORE_HPP__

‎launch/gnss_preprocessing.launch

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
<launch>
2+
<!-- tf satic -->
3+
<node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0 0 0 0 0 0 map odom 100" />
4+
<node pkg="tf" type="static_transform_publisher" name="baselink_to_navsat" args="0 0 0 0 0 0 base_link navsat 100" />
5+
<!--/nmea_sentence(nmea_msgs/Sentence) convert to /fix(sensor_msgs/NavSatFix)-->
6+
<node pkg="nmea_navsat_driver" type="nmea_topic_driver" name="nmea_topic_driver" output="screen"/>
7+
<!--/fix(sensor_msgs/NavSatFix) convert to /gps_path(nav_msgs/Path)-->
8+
<node pkg="gnss_preprocessing" name="gnss_preprocessing" type="gnss_preprocessing" output="screen"/>
9+
<!--rosbag play-->
10+
<!--<node pkg="rosbag" type="play" name="player" output="screen" args="-clock /home/ubuntu/Downloads/utbm_robocar_dataset_20190131_noimage.bag"/>-->
11+
<!--display rviz-->
12+
<!--<node name="rviz" pkg="rviz" type="rviz" args="-d $(find gnss_imu_odom_ESKF)/rviz/gps_trajectory_plotter.rviz" required="true"/>-->
13+
</launch>

‎src/gnss_preprocessing.cpp

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
#include <ros/ros.h>
2+
3+
//#include "gnss_preprocessing_component.hpp"
4+
#include "gnss_preprocessing_core.hpp"
5+
6+
int main(int argc, char**argv)
7+
{
8+
ros::init(argc, argv, "gnss_preprocessing");
9+
ros::NodeHandle n;
10+
11+
double lat0 = 36.08263197663968;
12+
double lon0 = 140.07660776463374;
13+
double hig0 = 0.0;
14+
15+
//GnssPreprocessingComponent gnss_preprocessing_component(n);
16+
17+
GnssPreprocessingCore gnss_preprocessing_core();
18+
19+
while(ros::ok()){
20+
//std::cout << "ROS_Interface Start!" << std::endl;
21+
ros::spinOnce();
22+
}
23+
24+
return 0;
25+
}

0 commit comments

Comments
 (0)