cartographer_ros: msg_conversion

Go to the documentation of this file.
00001 /*
00002  * Copyright 2016 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include "cartographer_ros/msg_conversion.h"
00018 
00019 #include <cmath>
00020 #include <random>
00021 
00022 #include "cartographer/transform/rigid_transform_test_helpers.h"
00023 #include "cartographer_ros/time_conversion.h"
00024 #include "gmock/gmock.h"
00025 #include "gtest/gtest.h"
00026 #include "sensor_msgs/LaserScan.h"
00027 
00028 namespace cartographer_ros {
00029 namespace {
00030 
00031 using ::cartographer::sensor::LandmarkData;
00032 using ::cartographer::sensor::LandmarkObservation;
00033 using ::testing::AllOf;
00034 using ::testing::DoubleNear;
00035 using ::testing::ElementsAre;
00036 using ::testing::Field;
00037 
00038 constexpr double kEps = 1e-6;
00039 
00040 TEST(MsgConversion, LaserScanToPointCloud) {
00041   sensor_msgs::LaserScan laser_scan;
00042   for (int i = 0; i < 8; ++i) {
00043     laser_scan.ranges.push_back(1.f);
00044   }
00045   laser_scan.angle_min = 0.f;
00046   laser_scan.angle_max = 8.f * static_cast<float>(M_PI_4);
00047   laser_scan.angle_increment = static_cast<float>(M_PI_4);
00048   laser_scan.range_min = 0.f;
00049   laser_scan.range_max = 10.f;
00050 
00051   const auto point_cloud =
00052       std::get<0>(ToPointCloudWithIntensities(laser_scan)).points;
00053   EXPECT_TRUE(
00054       point_cloud[0].position.isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), kEps));
00055   EXPECT_TRUE(point_cloud[1].position.isApprox(
00056       Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps));
00057   EXPECT_TRUE(
00058       point_cloud[2].position.isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), kEps));
00059   EXPECT_TRUE(point_cloud[3].position.isApprox(
00060       Eigen::Vector3f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps));
00061   EXPECT_TRUE(
00062       point_cloud[4].position.isApprox(Eigen::Vector3f(-1.f, 0.f, 0.f), kEps));
00063   EXPECT_TRUE(point_cloud[5].position.isApprox(
00064       Eigen::Vector3f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f),
00065       kEps));
00066   EXPECT_TRUE(
00067       point_cloud[6].position.isApprox(Eigen::Vector3f(0.f, -1.f, 0.f), kEps));
00068   EXPECT_TRUE(point_cloud[7].position.isApprox(
00069       Eigen::Vector3f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), kEps));
00070   for (int i = 0; i < 8; ++i) {
00071     EXPECT_NEAR(point_cloud[i].time, 0.f, kEps);
00072   }
00073 }
00074 
00075 TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) {
00076   sensor_msgs::LaserScan laser_scan;
00077   laser_scan.ranges.push_back(1.f);
00078   laser_scan.ranges.push_back(std::numeric_limits<float>::infinity());
00079   laser_scan.ranges.push_back(2.f);
00080   laser_scan.ranges.push_back(std::numeric_limits<float>::quiet_NaN());
00081   laser_scan.ranges.push_back(3.f);
00082   laser_scan.angle_min = 0.f;
00083   laser_scan.angle_max = 3.f * static_cast<float>(M_PI_4);
00084   laser_scan.angle_increment = static_cast<float>(M_PI_4);
00085   laser_scan.range_min = 2.f;
00086   laser_scan.range_max = 10.f;
00087 
00088   const auto point_cloud =
00089       std::get<0>(ToPointCloudWithIntensities(laser_scan)).points;
00090   ASSERT_EQ(2, point_cloud.size());
00091   EXPECT_TRUE(
00092       point_cloud[0].position.isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), kEps));
00093   EXPECT_TRUE(
00094       point_cloud[1].position.isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), kEps));
00095   EXPECT_NEAR(point_cloud[0].time, 0.f, kEps);
00096   EXPECT_NEAR(point_cloud[1].time, 0.f, kEps);
00097 }
00098 
00099 ::testing::Matcher<const LandmarkObservation&> EqualsLandmark(
00100     const LandmarkObservation& expected) {
00101   return ::testing::AllOf(
00102       Field(&LandmarkObservation::id, expected.id),
00103       Field(&LandmarkObservation::landmark_to_tracking_transform,
00104             ::cartographer::transform::IsNearly(
00105                 expected.landmark_to_tracking_transform, kEps)),
00106       Field(&LandmarkObservation::translation_weight,
00107             DoubleNear(expected.translation_weight, kEps)),
00108       Field(&LandmarkObservation::rotation_weight,
00109             DoubleNear(expected.rotation_weight, kEps)));
00110 }
00111 
00112 TEST(MsgConversion, LandmarkListToLandmarkData) {
00113   cartographer_ros_msgs::LandmarkList message;
00114   message.header.stamp.fromSec(10);
00115 
00116   cartographer_ros_msgs::LandmarkEntry landmark_0;
00117   landmark_0.id = "landmark_0";
00118   landmark_0.tracking_from_landmark_transform.position.x = 1.0;
00119   landmark_0.tracking_from_landmark_transform.position.y = 2.0;
00120   landmark_0.tracking_from_landmark_transform.position.z = 3.0;
00121   landmark_0.tracking_from_landmark_transform.orientation.w = 1.0;
00122   landmark_0.tracking_from_landmark_transform.orientation.x = 0.0;
00123   landmark_0.tracking_from_landmark_transform.orientation.y = 0.0;
00124   landmark_0.tracking_from_landmark_transform.orientation.z = 0.0;
00125   landmark_0.translation_weight = 1.0;
00126   landmark_0.rotation_weight = 2.0;
00127   message.landmarks.push_back(landmark_0);
00128 
00129   LandmarkData actual_landmark_data = ToLandmarkData(message);
00130   EXPECT_THAT(actual_landmark_data,
00131               AllOf(Field(&LandmarkData::time, FromRos(message.header.stamp)),
00132                     Field(&LandmarkData::landmark_observations,
00133                           ElementsAre(EqualsLandmark(LandmarkObservation{
00134                               "landmark_0",
00135                               ::cartographer::transform::Rigid3d(
00136                                   Eigen::Vector3d(1., 2., 3.),
00137                                   Eigen::Quaterniond(1., 0., 0., 0.)),
00138                               1.f,
00139                               2.f,
00140                           })))));
00141 }
00142 
00143 TEST(MsgConversion, LatLongAltToEcef) {
00144   Eigen::Vector3d equator_prime_meridian = LatLongAltToEcef(0, 0, 0);
00145   EXPECT_TRUE(equator_prime_meridian.isApprox(Eigen::Vector3d(6378137, 0, 0)))
00146       << equator_prime_meridian;
00147   Eigen::Vector3d plus_ten_meters = LatLongAltToEcef(0, 0, 10);
00148   EXPECT_TRUE(plus_ten_meters.isApprox(Eigen::Vector3d(6378147, 0, 0)))
00149       << plus_ten_meters;
00150   Eigen::Vector3d north_pole = LatLongAltToEcef(90, 0, 0);
00151   EXPECT_TRUE(north_pole.isApprox(Eigen::Vector3d(0, 0, 6356752.3142), kEps))
00152       << north_pole;
00153   Eigen::Vector3d also_north_pole = LatLongAltToEcef(90, 90, 0);
00154   EXPECT_TRUE(
00155       also_north_pole.isApprox(Eigen::Vector3d(0, 0, 6356752.3142), kEps))
00156       << also_north_pole;
00157   Eigen::Vector3d south_pole = LatLongAltToEcef(-90, 0, 0);
00158   EXPECT_TRUE(south_pole.isApprox(Eigen::Vector3d(0, 0, -6356752.3142), kEps))
00159       << south_pole;
00160   Eigen::Vector3d above_south_pole = LatLongAltToEcef(-90, 60, 20);
00161   EXPECT_TRUE(
00162       above_south_pole.isApprox(Eigen::Vector3d(0, 0, -6356772.3142), kEps))
00163       << above_south_pole;
00164   Eigen::Vector3d somewhere_on_earth =
00165       LatLongAltToEcef(48.1372149, 11.5748024, 517.1);
00166   EXPECT_TRUE(somewhere_on_earth.isApprox(
00167       Eigen::Vector3d(4177983, 855702, 4727457), kEps))
00168       << somewhere_on_earth;
00169 }
00170 
00171 TEST(MsgConversion, ComputeLocalFrameFromLatLong) {
00172   cartographer::transform::Rigid3d north_pole =
00173       ComputeLocalFrameFromLatLong(90., 0.);
00174   EXPECT_TRUE((north_pole * LatLongAltToEcef(90., 0., 1.))
00175                   .isApprox(Eigen::Vector3d::UnitZ()));
00176   cartographer::transform::Rigid3d south_pole =
00177       ComputeLocalFrameFromLatLong(-90., 0.);
00178   EXPECT_TRUE((south_pole * LatLongAltToEcef(-90., 0., 1.))
00179                   .isApprox(Eigen::Vector3d::UnitZ()));
00180   cartographer::transform::Rigid3d prime_meridian_equator =
00181       ComputeLocalFrameFromLatLong(0., 0.);
00182   EXPECT_TRUE((prime_meridian_equator * LatLongAltToEcef(0., 0., 1.))
00183                   .isApprox(Eigen::Vector3d::UnitZ()))
00184       << prime_meridian_equator * LatLongAltToEcef(0., 0., 1.);
00185   cartographer::transform::Rigid3d meridian_90_equator =
00186       ComputeLocalFrameFromLatLong(0., 90.);
00187   EXPECT_TRUE((meridian_90_equator * LatLongAltToEcef(0., 90., 1.))
00188                   .isApprox(Eigen::Vector3d::UnitZ()))
00189       << meridian_90_equator * LatLongAltToEcef(0., 90., 1.);
00190 
00191   std::mt19937 rng(42);
00192   std::uniform_real_distribution<double> lat_distribution(-90., 90.);
00193   std::uniform_real_distribution<double> long_distribution(-180., 180.);
00194   std::uniform_real_distribution<double> alt_distribution(-519., 519.);
00195 
00196   for (int i = 0; i < 1000; ++i) {
00197     const double latitude = lat_distribution(rng);
00198     const double longitude = long_distribution(rng);
00199     const double altitude = alt_distribution(rng);
00200     cartographer::transform::Rigid3d transform_to_local_frame =
00201         ComputeLocalFrameFromLatLong(latitude, longitude);
00202     EXPECT_TRUE((transform_to_local_frame *
00203                  LatLongAltToEcef(latitude, longitude, altitude))
00204                     .isApprox(altitude * Eigen::Vector3d::UnitZ(), kEps))
00205         << transform_to_local_frame *
00206                LatLongAltToEcef(latitude, longitude, altitude)
00207         << "\n"
00208         << altitude;
00209   }
00210 }
00211 
00212 }  // namespace
00213 }  // namespace cartographer_ros