[RB-1640] Add config option to invert base_link to map tf

Dockerfile.base.melodic

@@ -0,0 +1,43 @@
+# Copyright 2018 The Cartographer Authors
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+FROM ros:melodic
+
+ARG CARTOGRAPHER_VERSION=master
+
+# Bionic'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 \
+    cartographer_ros/cartographer_version.json
+
+# wstool needs the updated rosinstall file to clone the correct repos.
+COPY cartographer_ros.rosinstall cartographer_ros/
+COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/
+RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \
+    cartographer_ros/scripts/prepare_catkin_workspace.sh
+
+# rosdep needs the updated package.xml files to install the correct debs.
+COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/
+COPY cartographer_ros_msgs/package.xml catkin_ws/src/cartographer_ros/cartographer_ros_msgs/
+COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_rviz/
+COPY scripts/install_debs.sh cartographer_ros/scripts/
+RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/*
+
+# Install proto3.
+RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh
+

Dockerfile.melodic

@@ -12,7 +12,7 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-FROM ros:melodic
+FROM eu.gcr.io/cartographer-141408/cartographer_ros_melodic_base
 
 ARG CARTOGRAPHER_VERSION=master
 
@@ -30,9 +30,9 @@ ADD https://api.github.com/repos/googlecartographer/cartographer/git/refs/heads/
 
 # wstool needs the updated rosinstall file to clone the correct repos.
 COPY cartographer_ros.rosinstall cartographer_ros/
-COPY scripts/prepare_catkin_workspace.sh cartographer_ros/scripts/
+COPY scripts/update_catkin_workspace.sh cartographer_ros/scripts/
 RUN CARTOGRAPHER_VERSION=$CARTOGRAPHER_VERSION \
-    cartographer_ros/scripts/prepare_catkin_workspace.sh
+    cartographer_ros/scripts/update_catkin_workspace.sh
 
 # rosdep needs the updated package.xml files to install the correct debs.
 COPY cartographer_ros/package.xml catkin_ws/src/cartographer_ros/cartographer_ros/
@@ -41,9 +41,6 @@ COPY cartographer_rviz/package.xml catkin_ws/src/cartographer_ros/cartographer_r
 COPY scripts/install_debs.sh cartographer_ros/scripts/
 RUN cartographer_ros/scripts/install_debs.sh && rm -rf /var/lib/apt/lists/*
 
-# Install proto3.
-RUN /catkin_ws/src/cartographer/scripts/install_proto3.sh
-
 # Build, install, and test all packages individually to allow caching. The
 # ordering of these steps must match the topological package ordering as
 # determined by Catkin.

azure-pipelines.yml

@@ -0,0 +1,70 @@
+# Copyright 2018 The Cartographer Authors
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+jobs:
+- job: Build
+  pool:
+    vmImage: 'vs2017-win2016'
+  timeoutInMinutes: 360
+  steps:
+  - script: |
+      choco sources add -n=roswin -s https://roswin.azurewebsites.net/api/v2/ --priority 1
+      rem Azure VM runs out of space on C:, so use D: for ros and rosdeps
+      mkdir D:\opt && mklink /J C:\opt D:\opt
+      choco upgrade %ROS_METAPACKAGE% -y
+      choco upgrade ros-melodic-perception -y
+      robocopy "." ".\src\cartographer_ros" /E /MOVE /XD "src" > NUL
+      git clone https://github.com/googlecartographer/cartographer src\cartographer
+      call "C:\opt\ros\melodic\x64\env.bat" rosdep install --from-paths src --ignore-src -r -y
+    env:
+      ROS_METAPACKAGE: 'ros-melodic-desktop'
+    displayName: Install prerequisites
+
+  - script: |
+      call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat"
+      call "C:\opt\ros\melodic\x64\setup.bat"
+      call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat
+      catkin_make_isolated --use-ninja --install --cmake-args -DCMAKE_BUILD_TYPE=Release
+    displayName: Build
+
+  - script: |
+      call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat"
+      call "C:\opt\ros\melodic\x64\setup.bat"
+      call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat
+      cd build_isolated\cartographer\install && ctest --no-compress-output -T Test
+    displayName: Run cartographer tests
+
+  - script: |
+      call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat"
+      call "C:\opt\ros\melodic\x64\setup.bat"
+      call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat
+      cd build_isolated\cartographer_ros && ninja tests && ctest --no-compress-output -T Test
+    displayName: Build and run cartographer_ros tests
+    condition: always()
+
+  - script: |
+      call "C:\Program Files (x86)\Microsoft Visual Studio\2017\Enterprise\VC\Auxiliary\Build\vcvars64.bat"
+      call "C:\opt\ros\melodic\x64\setup.bat"
+      call src\cartographer\scripts\remove_mingw_cygwin_from_path.bat
+      python src\cartographer\scripts\ctest_to_junit.py build_isolated\cartographer_ros
+    displayName: Convert tests to jUnit
+    condition: always()
+
+  - task: PublishTestResults@2
+    displayName: Publish test results
+    inputs:
+      testRunner: 'jUnit'
+      testResultsFiles: '**\jUnit.xml'
+      searchFolder: '$(Build.SourcesDirectory)\build_isolated\cartographer_ros\Testing'
+    condition: always()

cartographer_ros/CMakeLists.txt

@@ -35,9 +35,18 @@ set(PACKAGE_DEPENDENCIES
   visualization_msgs
 )
 
+if(WIN32)
+  set(Boost_USE_STATIC_LIBS FALSE)
+endif()
+# For yet unknown reason, if Boost is find_packaged() after find_package(cartographer),
+# some Boost libraries including Thread are set to static, despite Boost_USE_STATIC_LIBS,
+# which causes linking problems on windows due to shared/static Thread clashing.
+# PCL also finds Boost. Work around by moving before find_package(cartographer).
+find_package(Boost REQUIRED COMPONENTS system iostreams)
+find_package(PCL REQUIRED COMPONENTS common)
+
 find_package(cartographer REQUIRED)
 include("${CARTOGRAPHER_CMAKE_DIR}/functions.cmake")
-set(BUILD_SHARED_LIBS OFF)
 option(BUILD_GRPC "build features that require Cartographer gRPC support" false)
 google_initialize_cartographer_project()
 google_enable_testing()
@@ -46,10 +55,9 @@ find_package(catkin REQUIRED COMPONENTS ${PACKAGE_DEPENDENCIES})
 
 include(FindPkgConfig)
 
+find_package(Abseil REQUIRED)
 find_package(LuaGoogle REQUIRED)
-find_package(PCL REQUIRED COMPONENTS common)
 find_package(Eigen3 REQUIRED)
-find_package(Boost REQUIRED COMPONENTS system iostreams)
 
 find_package(urdfdom_headers REQUIRED)
 if(DEFINED urdfdom_headers_VERSION)
@@ -82,9 +90,9 @@ catkin_package(
   LIBRARIES ${PROJECT_NAME}
 )
 
-file(GLOB_RECURSE ALL_SRCS "*.cc" "*.h")
-file(GLOB_RECURSE ALL_TESTS "*_test.cc")
-file(GLOB_RECURSE ALL_EXECUTABLES "*_main.cc")
+file(GLOB_RECURSE ALL_SRCS "cartographer_ros/*.cc" "cartographer_ros/*.h")
+file(GLOB_RECURSE ALL_TESTS "cartographer_ros/*_test.cc")
+file(GLOB_RECURSE ALL_EXECUTABLES "cartographer_ros/*_main.cc")
 file(GLOB_RECURSE ALL_GRPC_FILES "cartographer_ros/cartographer_grpc/*")
 list(REMOVE_ITEM ALL_SRCS ${ALL_TESTS})
 list(REMOVE_ITEM ALL_SRCS ${ALL_EXECUTABLES})
@@ -93,7 +101,7 @@ if (NOT ${BUILD_GRPC})
   list(REMOVE_ITEM ALL_TESTS ${ALL_GRPC_FILES})
   list(REMOVE_ITEM ALL_EXECUTABLES ${ALL_GRPC_FILES})
 endif()
-add_library(${PROJECT_NAME} ${ALL_SRCS})
+add_library(${PROJECT_NAME} STATIC ${ALL_SRCS})
 add_subdirectory("cartographer_ros")
 
 target_link_libraries(${PROJECT_NAME} PUBLIC cartographer)
@@ -153,6 +161,10 @@ if (CATKIN_ENABLE_TESTING)
     target_link_libraries(${TEST_NAME} cartographer)
     target_link_libraries(${TEST_NAME} ${PROJECT_NAME})
     set_target_properties(${TEST_NAME} PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS})
+    # Needed for dynamically linked GTest on Windows.
+    if (WIN32)
+      target_compile_definitions(${TEST_NAME} PUBLIC -DGTEST_LINKED_AS_SHARED_LIBRARY)
+    endif()
   endforeach()
 endif()
 
@@ -171,7 +183,7 @@ install(TARGETS ${PROJECT_NAME}
 )
 
 # Install source headers.
-file(GLOB_RECURSE HDRS "*.h")
+file(GLOB_RECURSE HDRS "cartographer_ros/*.h")
 foreach(HDR ${HDRS})
   file(RELATIVE_PATH REL_FIL ${PROJECT_SOURCE_DIR} ${HDR})
   get_filename_component(INSTALL_DIR ${REL_FIL} DIRECTORY)

cartographer_ros/cartographer_ros/.clang-format

@@ -0,0 +1,2 @@
+BasedOnStyle: Google
+DerivePointerAlignment: false

cartographer_ros/cartographer_ros/CMakeLists.txt

@@ -47,17 +47,6 @@ install(TARGETS cartographer_offline_node
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
-google_binary(cartographer_start_trajectory
-  SRCS
-    start_trajectory_main.cc
-)
-
-install(TARGETS cartographer_start_trajectory
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
 google_binary(cartographer_occupancy_grid_node
   SRCS
     occupancy_grid_node_main.cc
@@ -102,6 +91,17 @@ install(TARGETS cartographer_pbstream_map_publisher
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
+google_binary(cartographer_dev_pbstream_trajectories_to_rosbag
+  SRCS
+  dev/pbstream_trajectories_to_rosbag_main.cc
+)
+
+install(TARGETS cartographer_dev_pbstream_trajectories_to_rosbag
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
 google_binary(cartographer_dev_rosbag_publisher
   SRCS
   dev/rosbag_publisher_main.cc

cartographer_ros/cartographer_ros/assets_writer.cc

@@ -20,8 +20,8 @@
 #include <fstream>
 #include <iostream>
 
+#include "absl/memory/memory.h"
 #include "cartographer/common/configuration_file_resolver.h"
-#include "cartographer/common/make_unique.h"
 #include "cartographer/common/math.h"
 #include "cartographer/io/file_writer.h"
 #include "cartographer/io/points_processor.h"
@@ -35,7 +35,6 @@
 #include "cartographer/transform/transform_interpolation_buffer.h"
 #include "cartographer_ros/msg_conversion.h"
 #include "cartographer_ros/ros_map_writing_points_processor.h"
-#include "cartographer_ros/split_string.h"
 #include "cartographer_ros/time_conversion.h"
 #include "cartographer_ros/urdf_reader.h"
 #include "gflags/gflags.h"
@@ -61,8 +60,7 @@ CreatePipelineBuilder(
     const std::string file_prefix) {
   const auto file_writer_factory =
       AssetsWriter::CreateFileWriterFactory(file_prefix);
-  auto builder =
-      carto::common::make_unique<carto::io::PointsProcessorPipelineBuilder>();
+  auto builder = absl::make_unique<carto::io::PointsProcessorPipelineBuilder>();
   carto::io::RegisterBuiltInPointsProcessors(trajectories, file_writer_factory,
                                              builder.get());
   builder->Register(RosMapWritingPointsProcessor::kConfigurationFileActionName,
@@ -80,13 +78,13 @@ std::unique_ptr<carto::common::LuaParameterDictionary> LoadLuaDictionary(
     const std::string& configuration_directory,
     const std::string& configuration_basename) {
   auto file_resolver =
-      carto::common::make_unique<carto::common::ConfigurationFileResolver>(
+      absl::make_unique<carto::common::ConfigurationFileResolver>(
           std::vector<std::string>{configuration_directory});
 
   const std::string code =
       file_resolver->GetFileContentOrDie(configuration_basename);
   auto lua_parameter_dictionary =
-      carto::common::make_unique<carto::common::LuaParameterDictionary>(
+      absl::make_unique<carto::common::LuaParameterDictionary>(
           code, std::move(file_resolver));
   return lua_parameter_dictionary;
 }
@@ -99,7 +97,7 @@ std::unique_ptr<carto::io::PointsBatch> HandleMessage(
         transform_interpolation_buffer) {
   const carto::common::Time start_time = FromRos(message.header.stamp);
 
-  auto points_batch = carto::common::make_unique<carto::io::PointsBatch>();
+  auto points_batch = absl::make_unique<carto::io::PointsBatch>();
   points_batch->start_time = start_time;
   points_batch->frame_id = message.header.frame_id;
 
@@ -111,7 +109,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;
     }
@@ -122,8 +121,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();
@@ -251,7 +251,7 @@ void AssetsWriter::Run(const std::string& configuration_directory,
           delayed_messages.pop_front();
         }
         delayed_messages.push_back(message);
-        LOG_EVERY_N(INFO, 100000)
+        LOG_EVERY_N(INFO, 10000)
             << "Processed " << (message.getTime() - begin_time).toSec()
             << " of " << duration_in_seconds << " bag time seconds...";
       }
@@ -264,8 +264,7 @@ void AssetsWriter::Run(const std::string& configuration_directory,
 ::cartographer::io::FileWriterFactory AssetsWriter::CreateFileWriterFactory(
     const std::string& file_path) {
   const auto file_writer_factory = [file_path](const std::string& filename) {
-    return carto::common::make_unique<carto::io::StreamFileWriter>(file_path +
-                                                                   filename);
+    return absl::make_unique<carto::io::StreamFileWriter>(file_path + filename);
   };
   return file_writer_factory;
 }

cartographer_ros/cartographer_ros/assets_writer_main.cc

@@ -14,8 +14,8 @@
  * limitations under the License.
  */
 
+#include "absl/strings/str_split.h"
 #include "cartographer_ros/assets_writer.h"
-#include "cartographer_ros/split_string.h"
 #include "gflags/gflags.h"
 #include "glog/logging.h"
 
@@ -56,7 +56,7 @@ int main(int argc, char** argv) {
 
   ::cartographer_ros::AssetsWriter asset_writer(
       FLAGS_pose_graph_filename,
-      cartographer_ros::SplitString(FLAGS_bag_filenames, ','),
+      absl::StrSplit(FLAGS_bag_filenames, ',', absl::SkipEmpty()),
       FLAGS_output_file_prefix);
 
   asset_writer.Run(FLAGS_configuration_directory, FLAGS_configuration_basename,