Gazebo Msgs

API Reference

10.1.1
SphericalCoordinates.hh
Go to the documentation of this file.
1/*
2 * Copyright (C) 2023 Open Source Robotics Foundation
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *
16 */
17#ifndef GZ_MSGS_CONVERT_SPHERICALCOORDINATES_HH_
18#define GZ_MSGS_CONVERT_SPHERICALCOORDINATES_HH_
19
20// Message Headers
21#include "gz/msgs/spherical_coordinates.pb.h"
22
23// Data Headers
24#include <gz/math/SphericalCoordinates.hh>
25
26namespace gz::msgs {
27// Inline bracket to help doxygen filtering.
28inline namespace GZ_MSGS_VERSION_NAMESPACE {
29
30
32inline msgs::SphericalCoordinatesType ConvertCoord(
34{
35 auto result = msgs::SphericalCoordinatesType::LOCAL2;
36 switch (_sc)
37 {
38 case math::SphericalCoordinates::CoordinateType::ECEF:
39 result = msgs::SphericalCoordinatesType::ECEF;
40 break;
41 case math::SphericalCoordinates::CoordinateType::GLOBAL:
42 result = msgs::SphericalCoordinatesType::GLOBAL;
43 break;
44 case math::SphericalCoordinates::CoordinateType::SPHERICAL:
45 result = msgs::SphericalCoordinatesType::SPHERICAL;
46 break;
47 case math::SphericalCoordinates::CoordinateType::LOCAL:
48 result = msgs::SphericalCoordinatesType::LOCAL;
49 break;
50 case math::SphericalCoordinates::CoordinateType::LOCAL2:
51 result = msgs::SphericalCoordinatesType::LOCAL2;
52 break;
53 default:
54 std::cerr << "Invalid coordinate type passed" << std::endl;
55 }
56 return result;
57}
58
61 const msgs::SphericalCoordinatesType &_sc)
62{
63 switch (_sc)
64 {
65 case msgs::SphericalCoordinatesType::ECEF:
66 return math::SphericalCoordinates::CoordinateType::ECEF;
67 case msgs::SphericalCoordinatesType::GLOBAL:
68 return math::SphericalCoordinates::CoordinateType::GLOBAL;
69 case msgs::SphericalCoordinatesType::SPHERICAL:
70 return math::SphericalCoordinates::CoordinateType::SPHERICAL;
71 case msgs::SphericalCoordinatesType::LOCAL:
72 return math::SphericalCoordinates::CoordinateType::LOCAL;
73 case msgs::SphericalCoordinatesType::LOCAL2:
74 return math::SphericalCoordinates::CoordinateType::LOCAL2;
75 default:
76 std::cerr << "Invalid coordinate type passed" << std::endl;
77 }
78 return math::SphericalCoordinates::CoordinateType::LOCAL2;
79}
80
82inline void Set(gz::msgs::SphericalCoordinates *_msg,
84{
85 if (_data.Surface() == math::SphericalCoordinates::EARTH_WGS84)
86 {
87 _msg->set_surface_model(msgs::SphericalCoordinates::EARTH_WGS84);
88 }
89 else if (_data.Surface() == math::SphericalCoordinates::MOON_SCS)
90 {
91 _msg->set_surface_model(msgs::SphericalCoordinates::MOON_SCS);
92 }
93 else if (_data.Surface() ==
94 math::SphericalCoordinates::CUSTOM_SURFACE)
95 {
96 _msg->set_surface_model(
97 msgs::SphericalCoordinates::CUSTOM_SURFACE);
98 _msg->set_surface_axis_equatorial(_data.SurfaceAxisEquatorial());
99 _msg->set_surface_axis_polar(_data.SurfaceAxisPolar());
100 }
101 else
102 {
103 std::cerr << "Unrecognized spherical surface type ["
104 << _data.Surface()
105 << "]. Not populating message field." << std::endl;
106 }
107 _msg->set_latitude_deg(_data.LatitudeReference().Degree());
108 _msg->set_longitude_deg(_data.LongitudeReference().Degree());
109 _msg->set_elevation(_data.ElevationReference());
110 _msg->set_heading_deg(_data.HeadingOffset().Degree());
111}
112
114 const gz::msgs::SphericalCoordinates &_msg)
115{
116 if (_msg.surface_model() == msgs::SphericalCoordinates::EARTH_WGS84)
117 {
118 _data->SetSurface(math::SphericalCoordinates::EARTH_WGS84);
119 }
120 else if (_msg.surface_model() == msgs::SphericalCoordinates::MOON_SCS)
121 {
122 _data->SetSurface(math::SphericalCoordinates::MOON_SCS);
123 }
124 else if (_msg.surface_model() == msgs::SphericalCoordinates::CUSTOM_SURFACE)
125 {
126 _data->SetSurface(math::SphericalCoordinates::CUSTOM_SURFACE,
127 _msg.surface_axis_equatorial(),
128 _msg.surface_axis_polar());
129 }
130 else
131 {
132 std::cerr << "Unrecognized spherical surface type ["
133 << _msg.surface_model()
134 << "]. Not populating data field." << std::endl;
135 }
136
137 auto lat = gz::math::Angle();
138 lat.SetDegree(_msg.latitude_deg());
139 _data->SetLatitudeReference(lat);
140
141 auto lon = gz::math::Angle();
142 lon.SetDegree(_msg.longitude_deg());
143 _data->SetLongitudeReference(lon);
144
145 auto head = gz::math::Angle();
146 head.SetDegree(_msg.heading_deg());
147 _data->SetHeadingOffset(head);
148
149 _data->SetElevationReference(_msg.elevation());
150}
151
152inline gz::msgs::SphericalCoordinates
154{
155 gz::msgs::SphericalCoordinates ret;
156 Set(&ret, _data);
157 return ret;
158}
159
161 Convert(const gz::msgs::SphericalCoordinates &_msg)
162{
164 Set(&ret, _msg);
165 return ret;
166}
167} // namespce
168} // namespace gz::msgs
169#endif // GZ_MSGS_CONVERT_VECTOR3_HH_