37 #include <gtest/gtest.h> 49 geographic_msgs::GeoPoint pt;
53 EXPECT_NEAR(lat2, pt.latitude, epsilon);
54 EXPECT_NEAR(lon2, pt.longitude, epsilon);
62 TEST(GeoPoint, nullConstructor)
64 geographic_msgs::GeoPoint pt;
78 TEST(GeoPoint, fromNavSatFix)
83 sensor_msgs::NavSatFix fix;
93 EXPECT_EQ(pt.latitude, lat);
94 EXPECT_EQ(pt.longitude, lon);
95 EXPECT_EQ(pt.altitude, alt);
99 TEST(GeoPoint, hasAltitude)
104 pt.altitude = -100.0;
107 pt.altitude = 20000.0;
117 geographic_msgs::GeoPoint pt;
126 pt.longitude = -97.0;
129 pt.longitude = 179.999999;
132 pt.longitude = -180.0;
139 geographic_msgs::GeoPoint pt;
141 pt.latitude = 90.001;
148 pt.longitude = -1000.0;
151 pt.longitude = 180.0;
154 pt.longitude = 180.0001;
157 pt.longitude = -180.999;
232 geographic_msgs::GeoPose pose;
239 TEST(GeoPose, quaternionValidity)
241 geographic_msgs::GeoPose pose;
244 pose.orientation.x = 1.0;
247 pose.orientation.x = 0.7071;
248 pose.orientation.y = 0.7071;
251 pose.orientation.x = 0.8071;
252 pose.orientation.y = 0.8071;
257 TEST(Convert, GeoPointToGeoPoint)
259 geographic_msgs::GeoPoint pt1(
geodesy::toMsg(30.0, 206.0, -97.0));
260 geographic_msgs::GeoPoint pt2;
264 EXPECT_EQ(pt1.latitude, pt2.latitude);
265 EXPECT_EQ(pt1.longitude, pt2.longitude);
266 EXPECT_EQ(pt1.altitude, pt2.altitude);
270 TEST(Convert, GeoPoseToGeoPose)
273 geometry_msgs::Quaternion q;
275 geographic_msgs::GeoPose pose2;
279 EXPECT_EQ(pose1.position.latitude, pose2.position.latitude);
280 EXPECT_EQ(pose1.position.longitude, pose2.position.longitude);
281 EXPECT_EQ(pose1.position.altitude, pose2.position.altitude);
286 int main(
int argc,
char **argv)
288 testing::InitGoogleTest(&argc, argv);
291 return RUN_ALL_TESTS();
WGS 84 geodetic system for ROS latitude and longitude messages.
int main(int argc, char **argv)
TEST(GeoPoint, nullConstructor)
void check_normalize(double lat1, double lon1, double lat2, double lon2)
static bool is2D(const UTMPoint &pt)
static void normalize(UTMPoint &pt)
bool isValid(const UTMPoint &pt)
void convert(const From &from, To &to)
geographic_msgs::GeoPoint toMsg(const UTMPoint &from)