@@ -21,29 +21,35 @@ namespace interface
21
21
namespace gnss
22
22
{
23
23
24
- MappingUtils::MappingUtils () {}
24
+ MappingUtils::MappingUtils ()
25
+ {
26
+ }
25
27
26
- MappingUtils::~MappingUtils () {}
28
+ MappingUtils::~MappingUtils ()
29
+ {
30
+ }
27
31
28
- void MappingUtils::llaToxyz (const MappingParameters ¶ms, double &x_out, double &y_out, double &z_out) {
29
- if (params.proj_str .size () < 8 ) return ;
32
+ void MappingUtils::llaToxyz (
33
+ const MappingParameters & params, double & x_out, double & y_out, double & z_out)
34
+ {
35
+ if (params.proj_str .size () < 8 ) return ;
30
36
31
- PJ_CONTEXT *C = proj_context_create ();
32
- PJ *P = proj_create_crs_to_crs (C, " EPSG:4326" , params.proj_str .c_str (), NULL );
37
+ PJ_CONTEXT * C = proj_context_create ();
38
+ PJ * P = proj_create_crs_to_crs (C, " EPSG:4326" , params.proj_str .c_str (), NULL );
33
39
34
- if (P == nullptr ) {
35
- proj_context_destroy (C);
36
- return ;
37
- }
40
+ if (P == nullptr ) {
41
+ proj_context_destroy (C);
42
+ return ;
43
+ }
38
44
39
- PJ_COORD gps_degrees = proj_coord (params.lat , params.lon , params.alt , 0 );
40
- PJ_COORD xyz_out = proj_trans (P, PJ_FWD, gps_degrees);
41
- x_out = xyz_out.enu .e + params.org .pos .x ;
42
- y_out = xyz_out.enu .n + params.org .pos .y ;
43
- z_out = xyz_out.enu .u + params.org .pos .z ;
45
+ PJ_COORD gps_degrees = proj_coord (params.lat , params.lon , params.alt , 0 );
46
+ PJ_COORD xyz_out = proj_trans (P, PJ_FWD, gps_degrees);
47
+ x_out = xyz_out.enu .e + params.org .pos .x ;
48
+ y_out = xyz_out.enu .n + params.org .pos .y ;
49
+ z_out = xyz_out.enu .u + params.org .pos .z ;
44
50
45
- proj_destroy (P);
46
- proj_context_destroy (C);
51
+ proj_destroy (P);
52
+ proj_context_destroy (C);
47
53
}
48
54
} // namespace gnss
49
55
} // namespace interface
@@ -58,7 +64,8 @@ void GnssInterface::GnssCallBack(const sensor_msgs::msg::NavSatFix::SharedPtr ms
58
64
59
65
// Create MappingParameters struct to hold the parameters
60
66
interface::gnss::MappingParameters params;
61
- params.proj_str = " +proj=tmerc +lat_0=0 +lon_0=0 +k=0.9999 +x_0=0 +y_0=0 +ellps=GRS80 +units=m +no_defs" ;
67
+ params.proj_str =
68
+ " +proj=tmerc +lat_0=0 +lon_0=0 +k=0.9999 +x_0=0 +y_0=0 +ellps=GRS80 +units=m +no_defs" ;
62
69
params.lat = msg->latitude ;
63
70
params.lon = msg->longitude ;
64
71
params.alt = msg->altitude ;
0 commit comments