Set up CI with Azure Pipelines

Dockerfile.indigo

@@ -16,13 +16,10 @@ FROM ros:indigo
 
 ARG CARTOGRAPHER_VERSION=master
 
-# We require a GitHub access token to be passed.
-ARG github_token
-
 # First, we invalidate the entire cache if googlecartographer/cartographer has
 # changed. This file's content changes whenever master changes. See:
 # http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone
-ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \
+ADD https://api.github.com/repos/larics/cartographer/git/refs/heads/master \
     cartographer_ros/cartographer_version.json
 
 # wstool needs the updated rosinstall file to clone the correct repos.

Dockerfile.kinetic

@@ -16,16 +16,13 @@ FROM ros:kinetic
 
 ARG CARTOGRAPHER_VERSION=master
 
-# We require a GitHub access token to be passed.
-ARG github_token
-
 # Xenial's base image doesn't ship with sudo.
 RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/*
 
 # First, we invalidate the entire cache if googlecartographer/cartographer has
 # changed. This file's content changes whenever master changes. See:
 # http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone
-ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \
+ADD https://api.github.com/repos/larics/cartographer/git/refs/heads/master \
     cartographer_ros/cartographer_version.json
 
 # wstool needs the updated rosinstall file to clone the correct repos.

Dockerfile.lunar

@@ -16,16 +16,13 @@ FROM ros:lunar
 
 ARG CARTOGRAPHER_VERSION=master
 
-# We require a GitHub access token to be passed.
-ARG github_token
-
 # Xenial's base image doesn't ship with sudo.
 RUN apt-get update && apt-get install -y sudo && rm -rf /var/lib/apt/lists/*
 
 # First, we invalidate the entire cache if googlecartographer/cartographer has
 # changed. This file's content changes whenever master changes. See:
 # http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone
-ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master?access_token=$github_token \
+ADD https://api.github.com/repos/larics/cartographer/git/refs/heads/master \
     cartographer_ros/cartographer_version.json
 
 # wstool needs the updated rosinstall file to clone the correct repos.

azure-pipelines.yml

@@ -0,0 +1,16 @@
+# Starter pipeline
+# Start with a minimal pipeline that you can customize to build and deploy your code.
+# Add steps that build, run tests, deploy, and more:
+# https://aka.ms/yaml
+
+pool:
+  vmImage: 'Ubuntu 16.04'
+
+steps:
+- script: echo Hello, world!
+  displayName: 'Run a one-line script'
+
+- script: |
+    echo Add other tasks to build, test, and deploy your project.
+    echo See https://aka.ms/yaml
+  displayName: 'Run a multi-line script'

cartographer_ros.rosinstall

@@ -1,3 +1,3 @@
-- git: {local-name: cartographer, uri: 'https://github.com/googlecartographer/cartographer.git', version: '1.0.0'}
-- git: {local-name: cartographer_ros, uri: 'https://github.com/googlecartographer/cartographer_ros.git', version: '1.0.0'}
+- git: {local-name: cartographer, uri: 'https://github.com/larics/cartographer.git'}
+- git: {local-name: cartographer_ros, uri: 'https://github.com/googlecartographer/cartographer_ros.git'}
 - git: {local-name: ceres-solver, uri: 'https://ceres-solver.googlesource.com/ceres-solver.git', version: '1.13.0'}

cartographer_ros/cartographer_ros/assets_writer.cc

@@ -110,7 +110,8 @@ std::unique_ptr<carto::io::PointsBatch> HandleMessage(
 
   for (size_t i = 0; i < point_cloud.points.size(); ++i) {
     const carto::common::Time time =
-        point_cloud_time + carto::common::FromSeconds(point_cloud.points[i][3]);
+        point_cloud_time +
+        carto::common::FromSeconds(point_cloud.points[i].time);
     if (!transform_interpolation_buffer.Has(time)) {
       continue;
     }
@@ -121,8 +122,9 @@ std::unique_ptr<carto::io::PointsBatch> HandleMessage(
             tracking_frame, message.header.frame_id, ToRos(time)));
     const carto::transform::Rigid3f sensor_to_map =
         (tracking_to_map * sensor_to_tracking).cast<float>();
-    points_batch->points.push_back(sensor_to_map *
-                                   point_cloud.points[i].head<3>());
+    points_batch->points.push_back(
+        sensor_to_map *
+        carto::sensor::ToRangefinderPoint(point_cloud.points[i]));
     points_batch->intensities.push_back(point_cloud.intensities[i]);
     // We use the last transform for the origin, which is approximately correct.
     points_batch->origin = sensor_to_map * Eigen::Vector3f::Zero();

cartographer_ros/cartographer_ros/msg_conversion.cc

@@ -147,9 +147,9 @@ LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {
       const float first_echo = GetFirstEcho(echoes);
       if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
         const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
-        Eigen::Vector4f point;
-        point << rotation * (first_echo * Eigen::Vector3f::UnitX()),
-            i * msg.time_increment;
+        const cartographer::sensor::TimedRangefinderPoint point{
+            rotation * (first_echo * Eigen::Vector3f::UnitX()),
+            i * msg.time_increment};
         point_cloud.points.push_back(point);
         if (msg.intensities.size() > 0) {
           CHECK_EQ(msg.intensities.size(), msg.ranges.size());
@@ -165,10 +165,10 @@ LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {
   }
   ::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
   if (!point_cloud.points.empty()) {
-    const double duration = point_cloud.points.back()[3];
+    const double duration = point_cloud.points.back().time;
     timestamp += cartographer::common::FromSeconds(duration);
-    for (Eigen::Vector4f& point : point_cloud.points) {
-      point[3] -= duration;
+    for (auto& point : point_cloud.points) {
+      point.time -= duration;
     }
   }
   return std::make_tuple(point_cloud, timestamp);
@@ -191,10 +191,10 @@ sensor_msgs::PointCloud2 ToPointCloud2Message(
     const ::cartographer::sensor::TimedPointCloud& point_cloud) {
   auto msg = PreparePointCloud2Message(timestamp, frame_id, point_cloud.size());
   ::ros::serialization::OStream stream(msg.data.data(), msg.data.size());
-  for (const Eigen::Vector4f& point : point_cloud) {
-    stream.next(point.x());
-    stream.next(point.y());
-    stream.next(point.z());
+  for (const cartographer::sensor::TimedRangefinderPoint& point : point_cloud) {
+    stream.next(point.position.x());
+    stream.next(point.position.y());
+    stream.next(point.position.z());
     stream.next(kPointCloudComponentFourMagic);
   }
   return msg;
@@ -225,7 +225,8 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) {
       point_cloud.points.reserve(pcl_point_cloud.size());
       point_cloud.intensities.reserve(pcl_point_cloud.size());
       for (const auto& point : pcl_point_cloud) {
-        point_cloud.points.emplace_back(point.x, point.y, point.z, point.time);
+        point_cloud.points.push_back(
+            {Eigen::Vector3f{point.x, point.y, point.z}, point.time});
         point_cloud.intensities.push_back(point.intensity);
       }
     } else {
@@ -234,7 +235,8 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) {
       point_cloud.points.reserve(pcl_point_cloud.size());
       point_cloud.intensities.reserve(pcl_point_cloud.size());
       for (const auto& point : pcl_point_cloud) {
-        point_cloud.points.emplace_back(point.x, point.y, point.z, 0.f);
+        point_cloud.points.push_back(
+            {Eigen::Vector3f{point.x, point.y, point.z}, 0.f});
         point_cloud.intensities.push_back(point.intensity);
       }
     }
@@ -246,7 +248,8 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) {
       point_cloud.points.reserve(pcl_point_cloud.size());
       point_cloud.intensities.reserve(pcl_point_cloud.size());
       for (const auto& point : pcl_point_cloud) {
-        point_cloud.points.emplace_back(point.x, point.y, point.z, point.time);
+        point_cloud.points.push_back(
+            {Eigen::Vector3f{point.x, point.y, point.z}, point.time});
         point_cloud.intensities.push_back(1.0f);
       }
     } else {
@@ -255,19 +258,20 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) {
       point_cloud.points.reserve(pcl_point_cloud.size());
       point_cloud.intensities.reserve(pcl_point_cloud.size());
       for (const auto& point : pcl_point_cloud) {
-        point_cloud.points.emplace_back(point.x, point.y, point.z, 0.f);
+        point_cloud.points.push_back(
+            {Eigen::Vector3f{point.x, point.y, point.z}, 0.f});
         point_cloud.intensities.push_back(1.0f);
       }
     }
   }
   ::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
   if (!point_cloud.points.empty()) {
-    const double duration = point_cloud.points.back()[3];
+    const double duration = point_cloud.points.back().time;
     timestamp += cartographer::common::FromSeconds(duration);
-    for (Eigen::Vector4f& point : point_cloud.points) {
-      point[3] -= duration;
-      CHECK_LE(point[3], 0) << "Encountered a point with a larger stamp than "
-                               "the last point in the cloud.";
+    for (auto& point : point_cloud.points) {
+      point.time -= duration;
+      CHECK_LE(point.time, 0) << "Encountered a point with a larger stamp than "
+                                 "the last point in the cloud.";
     }
   }
   return std::make_tuple(point_cloud, timestamp);

cartographer_ros/cartographer_ros/msg_conversion_test.cc

@@ -50,25 +50,25 @@ TEST(MsgConversion, LaserScanToPointCloud) {
   const auto point_cloud =
       std::get<0>(ToPointCloudWithIntensities(laser_scan)).points;
   EXPECT_TRUE(
-      point_cloud[0].isApprox(Eigen::Vector4f(1.f, 0.f, 0.f, 0.f), kEps));
-  EXPECT_TRUE(point_cloud[1].isApprox(
-      Eigen::Vector4f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f, 0.f),
-      kEps));
+      point_cloud[0].position.isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), kEps));
+  EXPECT_TRUE(point_cloud[1].position.isApprox(
+      Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps));
   EXPECT_TRUE(
-      point_cloud[2].isApprox(Eigen::Vector4f(0.f, 1.f, 0.f, 0.f), kEps));
-  EXPECT_TRUE(point_cloud[3].isApprox(
-      Eigen::Vector4f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f, 0.f),
-      kEps));
+      point_cloud[2].position.isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), kEps));
+  EXPECT_TRUE(point_cloud[3].position.isApprox(
+      Eigen::Vector3f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), kEps));
   EXPECT_TRUE(
-      point_cloud[4].isApprox(Eigen::Vector4f(-1.f, 0.f, 0.f, 0.f), kEps));
-  EXPECT_TRUE(point_cloud[5].isApprox(
-      Eigen::Vector4f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f, 0.f),
+      point_cloud[4].position.isApprox(Eigen::Vector3f(-1.f, 0.f, 0.f), kEps));
+  EXPECT_TRUE(point_cloud[5].position.isApprox(
+      Eigen::Vector3f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f),
       kEps));
   EXPECT_TRUE(
-      point_cloud[6].isApprox(Eigen::Vector4f(0.f, -1.f, 0.f, 0.f), kEps));
-  EXPECT_TRUE(point_cloud[7].isApprox(
-      Eigen::Vector4f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f, 0.f),
-      kEps));
+      point_cloud[6].position.isApprox(Eigen::Vector3f(0.f, -1.f, 0.f), kEps));
+  EXPECT_TRUE(point_cloud[7].position.isApprox(
+      Eigen::Vector3f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), kEps));
+  for (int i = 0; i < 8; ++i) {
+    EXPECT_NEAR(point_cloud[i].time, 0.f, kEps);
+  }
 }
 
 TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) {
@@ -88,9 +88,11 @@ TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) {
       std::get<0>(ToPointCloudWithIntensities(laser_scan)).points;
   ASSERT_EQ(2, point_cloud.size());
   EXPECT_TRUE(
-      point_cloud[0].isApprox(Eigen::Vector4f(0.f, 2.f, 0.f, 0.f), kEps));
+      point_cloud[0].position.isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), kEps));
   EXPECT_TRUE(
-      point_cloud[1].isApprox(Eigen::Vector4f(-3.f, 0.f, 0.f, 0.f), kEps));
+      point_cloud[1].position.isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), kEps));
+  EXPECT_NEAR(point_cloud[0].time, 0.f, kEps);
+  EXPECT_NEAR(point_cloud[1].time, 0.f, kEps);
 }
 
 ::testing::Matcher<const LandmarkObservation&> EqualsLandmark(

cartographer_ros/cartographer_ros/node.cc

@@ -208,11 +208,10 @@ void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) {
         carto::sensor::TimedPointCloud point_cloud;
         point_cloud.reserve(trajectory_data.local_slam_data->range_data_in_local
                                 .returns.size());
-        for (const Eigen::Vector3f point :
+        for (const cartographer::sensor::RangefinderPoint point :
              trajectory_data.local_slam_data->range_data_in_local.returns) {
-          Eigen::Vector4f point_time;
-          point_time << point, 0.f;
-          point_cloud.push_back(point_time);
+          point_cloud.push_back(cartographer::sensor::ToTimedRangefinderPoint(
+              point, 0. /* time */));
         }
         scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
             carto::common::ToUniversal(trajectory_data.local_slam_data->time),

cartographer_ros/cartographer_ros/rosbag_validate_main.cc

@@ -219,14 +219,15 @@ class RangeDataChecker {
     const cartographer::sensor::TimedPointCloud& point_cloud =
         std::get<0>(point_cloud_time).points;
     *to = std::get<1>(point_cloud_time);
-    *from = *to + cartographer::common::FromSeconds(point_cloud[0][3]);
+    *from = *to + cartographer::common::FromSeconds(point_cloud[0].time);
     Eigen::Vector4f points_sum = Eigen::Vector4f::Zero();
-    for (const Eigen::Vector4f& point : point_cloud) {
-      points_sum += point;
+    for (const auto& point : point_cloud) {
+      points_sum.head<3>() += point.position;
+      points_sum[3] += point.time;
     }
     if (point_cloud.size() > 0) {
-      double first_point_relative_time = point_cloud[0][3];
-      double last_point_relative_time = (*point_cloud.rbegin())[3];
+      double first_point_relative_time = point_cloud[0].time;
+      double last_point_relative_time = (*point_cloud.rbegin()).time;
       if (first_point_relative_time > 0) {
         LOG_FIRST_N(ERROR, 1)
             << "Sensor with frame_id \"" << message.header.frame_id

cartographer_ros/cartographer_ros/sensor_bridge.cc

@@ -177,7 +177,7 @@ void SensorBridge::HandleLaserScan(
   if (points.points.empty()) {
     return;
   }
-  CHECK_LE(points.points.back()[3], 0);
+  CHECK_LE(points.points.back().time, 0);
   // TODO(gaschler): Use per-point time instead of subdivisions.
   for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
     const size_t start_index =
@@ -189,7 +189,7 @@ void SensorBridge::HandleLaserScan(
     if (start_index == end_index) {
       continue;
     }
-    const double time_to_subdivision_end = subdivision.back()[3];
+    const double time_to_subdivision_end = subdivision.back().time;
     // `subdivision_time` is the end of the measurement so sensor::Collator will
     // send all other sensor data first.
     const carto::common::Time subdivision_time =
@@ -204,10 +204,10 @@ void SensorBridge::HandleLaserScan(
       continue;
     }
     sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
-    for (Eigen::Vector4f& point : subdivision) {
-      point[3] -= time_to_subdivision_end;
+    for (auto& point : subdivision) {
+      point.time -= time_to_subdivision_end;
     }
-    CHECK_EQ(subdivision.back()[3], 0);
+    CHECK_EQ(subdivision.back().time, 0);
     HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
   }
 }
@@ -216,7 +216,7 @@ void SensorBridge::HandleRangefinder(
     const std::string& sensor_id, const carto::common::Time time,
     const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
   if (!ranges.empty()) {
-    CHECK_LE(ranges.back()[3], 0);
+    CHECK_LE(ranges.back().time, 0);
   }
   const auto sensor_to_tracking =
       tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));

jenkins/Dockerfile.kinetic

@@ -21,7 +21,7 @@ RUN apt-get update && apt-get install -y sudo time && rm -rf /var/lib/apt/lists/
 # First, we invalidate the entire cache if googlecartographer/cartographer has
 # changed. This file's content changes whenever master changes. See:
 # http://stackoverflow.com/questions/36996046/how-to-prevent-dockerfile-caching-git-clone
-ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/master \
+ADD https://api.github.com/repos/larics/cartographer/git/refs/heads/master \
     cartographer_ros/cartographer_version.json
 
 # wstool needs the updated rosinstall file to clone the correct repos.

jenkins/worker.py

@@ -104,7 +104,7 @@ def get_head_git_sha1():
   """Returns the SHA-1 hash of the commit tagged HEAD."""
   output = subprocess.check_output([
       'git', 'ls-remote',
-      'https://github.com/googlecartographer/cartographer.git'
+      'https://github.com/larics/cartographer.git'
   ])
   parsed = GIT_SHA1_PATTERN.extract(output)
   return parsed['sha1']