Gazebo Msgs

API Reference

10.3.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 
26 namespace gz::msgs {
27 // Inline bracket to help doxygen filtering.
28 inline namespace GZ_MSGS_VERSION_NAMESPACE {
29 
30 
32 inline msgs::SphericalCoordinatesType ConvertCoord(
33  const math::SphericalCoordinates::CoordinateType &_sc)
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 
60 inline math::SphericalCoordinates::CoordinateType Convert(
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 
82 inline void Set(gz::msgs::SphericalCoordinates *_msg,
83  const gz::math::SphericalCoordinates &_data)
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 
113 inline void Set(gz::math::SphericalCoordinates *_data,
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 
152 inline gz::msgs::SphericalCoordinates
153 Convert(const gz::math::SphericalCoordinates &_data)
154 {
155  gz::msgs::SphericalCoordinates ret;
156  Set(&ret, _data);
157  return ret;
158 }
159 
160 inline gz::math::SphericalCoordinates
161  Convert(const gz::msgs::SphericalCoordinates &_msg)
162 {
163  gz::math::SphericalCoordinates ret;
164  Set(&ret, _msg);
165  return ret;
166 }
167 } // namespce
168 } // namespace gz::msgs
169 #endif // GZ_MSGS_CONVERT_VECTOR3_HH_