Add code to allow offline processing of 2D Cartographer bags. (#1)

* Add code to allow offline processing of 2D Cartographer bags.

Fixes waiting for transforms to be published, adds support for multi-echo
laser scans, sets default values for the submap topic and query for the
RViz plugin.

cartographer_ros/CMakeLists.txt

@@ -97,10 +97,11 @@ target_link_libraries(cartographer_node
 add_library(cartographer_rviz_submaps_visualization
   src/drawable_submap.cc
   src/drawable_submap.h
+  src/node_constants.h
   src/submaps_display.cc
   src/submaps_display.h
-  src/trajectory.h
   src/trajectory.cc
+  src/trajectory.h
 )
 target_link_libraries(cartographer_rviz_submaps_visualization
   ${Boost_LIBRARIES}

cartographer_ros/configuration_files/backpack_2d.lua

@@ -0,0 +1,23 @@
+-- Copyright 2016 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.
+
+include "trajectory_builder.lua"
+include "sparse_pose_graph.lua"
+
+options = {
+  sparse_pose_graph = SPARSE_POSE_GRAPH,
+  trajectory_builder = TRAJECTORY_BUILDER,
+}
+
+return options

cartographer_ros/launch/mapping_2d.launch

@@ -0,0 +1,41 @@
+<!--
+  Copyright 2016 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.
+-->
+
+<launch>
+  <param name="robot_description"
+    textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" />
+
+  <node name="robot_state_publisher" pkg="robot_state_publisher"
+    type="robot_state_publisher" />
+
+  <node name="cartographer" pkg="cartographer_ros"
+      type="cartographer_node" output="screen" >
+    <rosparam subst_value="true">
+      map_frame: "map"
+      tracking_frame: "base_link"
+      configuration_files_directories: [
+        "$(find cartographer_ros)/configuration_files"
+      ]
+      mapping_configuration_basename: "backpack_2d.lua"
+      imu_topic: "/imu"
+      multi_echo_laser_scan_2d_topic: "/horizontal_2d_laser"
+      laser_min_range_m: 0.
+      laser_max_range_m: 30.
+      laser_missing_echo_ray_length_m: 5.
+    </rosparam>
+  </node>
+
+</launch>

cartographer_ros/src/cartographer_node_main.cc

@@ -42,14 +42,14 @@
 #include "cartographer/sensor/proto/sensor.pb.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
-#include "geometry_msgs/Transform.h"
-#include "geometry_msgs/TransformStamped.h"
-#include "glog/log_severity.h"
-#include "glog/logging.h"
 #include "cartographer_ros_msgs/SubmapEntry.h"
 #include "cartographer_ros_msgs/SubmapList.h"
 #include "cartographer_ros_msgs/SubmapQuery.h"
 #include "cartographer_ros_msgs/TrajectorySubmapList.h"
+#include "geometry_msgs/Transform.h"
+#include "geometry_msgs/TransformStamped.h"
+#include "glog/log_severity.h"
+#include "glog/logging.h"
 #include "pcl/point_cloud.h"
 #include "pcl/point_types.h"
 #include "pcl_conversions/pcl_conversions.h"
@@ -78,6 +78,7 @@ constexpr int64 kTrajectoryId = 0;
 constexpr int kSubscriberQueueSize = 150;
 constexpr int kSubmapPublishPeriodInUts = 300 * 10000ll;  // 300 milliseconds
 constexpr int kPosePublishPeriodInUts = 5 * 10000ll;      // 5 milliseconds
+constexpr double kMaxTransformDelaySeconds = 1.;
 
 Rigid3d ToRidig3d(const geometry_msgs::TransformStamped& transform) {
   return Rigid3d(Eigen::Vector3d(transform.transform.translation.x,
@@ -165,12 +166,10 @@ class Node {
   template <typename T>
   const T GetParamOrDie(const string& name);
 
-  // Returns true if a transform for 'frame_id' to 'tracking_frame_' exists at
+  // Returns a transform for 'frame_id' to 'tracking_frame_' if it exists at
-  // 'time'.
+  // 'time' or throws tf2::TransformException if it does not exist.
-  bool CanTransform(ros::Time time, const string& frame_id);
+  Rigid3d LookupToTrackingTransformOrThrow(::cartographer::common::Time time,
-
+                                           const string& frame_id);
-  Rigid3d LookupToTrackingTransformOrDie(ros::Time time,
-                                         const string& frame_id);
 
   bool HandleSubmapQuery(
       ::cartographer_ros_msgs::SubmapQuery::Request& request,
@@ -217,21 +216,11 @@ Node::Node()
       last_submap_list_publish_timestamp_(0),
       last_pose_publish_timestamp_(0) {}
 
-bool Node::CanTransform(ros::Time time, const string& frame_id) {
+Rigid3d Node::LookupToTrackingTransformOrThrow(
-  return tf_buffer_.canTransform(tracking_frame_, frame_id, time);
+    const ::cartographer::common::Time time, const string& frame_id) {
-}
+  return ToRidig3d(
-
+      tf_buffer_.lookupTransform(tracking_frame_, frame_id, ToRos(time),
-Rigid3d Node::LookupToTrackingTransformOrDie(ros::Time time,
+                                 ros::Duration(kMaxTransformDelaySeconds)));
-                                             const string& frame_id) {
-  geometry_msgs::TransformStamped stamped_transform;
-  try {
-    stamped_transform = tf_buffer_.lookupTransform(tracking_frame_, frame_id,
-                                                   time, ros::Duration(1.));
-  } catch (tf2::TransformException& ex) {
-    LOG(FATAL) << "Timed out while waiting for transform: " << frame_id
-               << " -> " << tracking_frame_ << ": " << ex.what();
-  }
-  return ToRidig3d(stamped_transform);
 }
 
 void Node::ImuMessageCallback(const sensor_msgs::Imu::ConstPtr& msg) {
@@ -247,22 +236,23 @@ void Node::AddImu(const int64 timestamp, const string& frame_id,
                   const proto::Imu& imu) {
   const ::cartographer::common::Time time =
       ::cartographer::common::FromUniversal(timestamp);
-
+  try {
-  if (!CanTransform(ToRos(time), frame_id)) {
+    const Rigid3d sensor_to_tracking =
-    LOG(WARNING) << "Cannot transform to " << frame_id;
+        LookupToTrackingTransformOrThrow(time, frame_id);
-    return;
+    CHECK(sensor_to_tracking.translation().norm() < 1e-5)
+        << "The IMU is not colocated with the tracking frame. This makes it "
+           "hard "
+           "and inprecise to transform its linear accelaration into the "
+           "tracking_frame and will decrease the quality of the SLAM.";
+    trajectory_builder_->AddImuData(
+        time, sensor_to_tracking.rotation() *
+                  ::cartographer::transform::ToEigen(imu.linear_acceleration()),
+        sensor_to_tracking.rotation() *
+            ::cartographer::transform::ToEigen(imu.angular_velocity()));
+  } catch (tf2::TransformException& ex) {
+    LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_
+                 << ": " << ex.what();
   }
-  const Rigid3d sensor_to_tracking =
-      LookupToTrackingTransformOrDie(ToRos(time), frame_id);
-  CHECK(sensor_to_tracking.translation().norm() < 1e-5)
-      << "The IMU is not colocated with the tracking frame. This makes it hard "
-         "and inprecise to transform its linear accelaration into the "
-         "tracking_frame and will decrease the quality of the SLAM.";
-  trajectory_builder_->AddImuData(
-      time, sensor_to_tracking.rotation() *
-                ::cartographer::transform::ToEigen(imu.linear_acceleration()),
-      sensor_to_tracking.rotation() *
-          ::cartographer::transform::ToEigen(imu.angular_velocity()));
 }
 
 void Node::LaserScanMessageCallback(
@@ -279,29 +269,33 @@ void Node::AddHorizontalLaserFan(const int64 timestamp, const string& frame_id,
                                  const proto::LaserScan& laser_scan) {
   const ::cartographer::common::Time time =
       ::cartographer::common::FromUniversal(timestamp);
-  if (!CanTransform(ToRos(time), frame_id)) {
+  try {
-    LOG(WARNING) << "Cannot transform to " << frame_id;
+    const Rigid3d sensor_to_tracking =
-    return;
+        LookupToTrackingTransformOrThrow(time, frame_id);
+    // TODO(hrapp): Make things configurable? Through Lua? Through ROS params?
+    const ::cartographer::sensor::LaserFan laser_fan =
+        ::cartographer::sensor::ToLaserFan(laser_scan, laser_min_range_m_,
+                                           laser_max_range_m_,
+                                           laser_missing_echo_ray_length_m_);
+
+    const auto laser_fan_3d = ::cartographer::sensor::TransformLaserFan3D(
+        ::cartographer::sensor::ToLaserFan3D(laser_fan),
+        sensor_to_tracking.cast<float>());
+    trajectory_builder_->AddHorizontalLaserFan(time, laser_fan_3d);
+  } catch (tf2::TransformException& ex) {
+    LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_
+                 << ": " << ex.what();
   }
-  const Rigid3d sensor_to_tracking =
-      LookupToTrackingTransformOrDie(ToRos(time), frame_id);
-
-  // TODO(hrapp): Make things configurable? Through Lua? Through ROS params?
-  const ::cartographer::sensor::LaserFan laser_fan =
-      ::cartographer::sensor::ToLaserFan(laser_scan, laser_min_range_m_,
-                                         laser_max_range_m_,
-                                         laser_missing_echo_ray_length_m_);
-
-  const auto laser_fan_3d = ::cartographer::sensor::TransformLaserFan3D(
-      ::cartographer::sensor::ToLaserFan3D(laser_fan),
-      sensor_to_tracking.cast<float>());
-  trajectory_builder_->AddHorizontalLaserFan(time, laser_fan_3d);
 }
 
 void Node::MultiEchoLaserScanCallback(
     const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
-  // TODO(hrapp): Do something useful.
+  auto sensor_data = ::cartographer::common::make_unique<SensorData>(
-  LOG(INFO) << "LaserScan message: " << msg->header.stamp;
+      msg->header.frame_id, ToCartographer(*msg));
+  sensor_collator_.AddSensorData(
+      kTrajectoryId,
+      ::cartographer::common::ToUniversal(FromRos(msg->header.stamp)),
+      laser_2d_subscriber_.getTopic(), std::move(sensor_data));
 }
 
 void Node::PointCloud2MessageCallback(
@@ -321,17 +315,17 @@ void Node::AddLaserFan3D(const int64 timestamp, const string& frame_id,
                          const proto::LaserFan3D& laser_fan_3d) {
   const ::cartographer::common::Time time =
       ::cartographer::common::FromUniversal(timestamp);
-  if (!CanTransform(ToRos(time), frame_id)) {
+  try {
-    LOG(WARNING) << "Cannot transform to " << frame_id;
+    const Rigid3d sensor_to_tracking =
-    return;
+        LookupToTrackingTransformOrThrow(time, frame_id);
+    trajectory_builder_->AddLaserFan3D(
+        time, ::cartographer::sensor::TransformLaserFan3D(
+                  ::cartographer::sensor::FromProto(laser_fan_3d),
+                  sensor_to_tracking.cast<float>()));
+  } catch (tf2::TransformException& ex) {
+    LOG(WARNING) << "Cannot transform " << frame_id << " -> " << tracking_frame_
+                 << ": " << ex.what();
   }
-  const Rigid3d sensor_to_tracking =
-      LookupToTrackingTransformOrDie(ToRos(time), frame_id);
-
-  trajectory_builder_->AddLaserFan3D(
-      time, ::cartographer::sensor::TransformLaserFan3D(
-                ::cartographer::sensor::FromProto(laser_fan_3d),
-                sensor_to_tracking.cast<float>()));
 }
 
 template <typename T>

cartographer_ros/src/drawable_submap.cc

@@ -19,8 +19,8 @@
 #include <OgreGpuProgramParams.h>
 #include <OgreImage.h>
 #include <cartographer/common/port.h>
-#include <eigen_conversions/eigen_msg.h>
 #include <cartographer_ros_msgs/SubmapQuery.h>
+#include <eigen_conversions/eigen_msg.h>
 #include <ros/ros.h>
 #include <rviz/display_context.h>
 #include <rviz/frame_manager.h>
@@ -117,8 +117,8 @@ void DrawableSubmap::QuerySubmap(const int submap_id, const int trajectory_id,
         srv.request.submap_id = submap_id;
         srv.request.trajectory_id = trajectory_id;
         if (client->call(srv)) {
-          response_.reset(new ::cartographer_ros_msgs::SubmapQuery::Response(
+          response_.reset(
-              srv.response));
+              new ::cartographer_ros_msgs::SubmapQuery::Response(srv.response));
           Q_EMIT RequestSucceeded();
         } else {
           OnRequestFailure();

cartographer_ros/src/msg_conversion.cc

@@ -240,6 +240,32 @@ sensor_msgs::PointCloud2 ToPointCloud2Message(
   return proto;
 }
 
+::cartographer::sensor::proto::LaserScan ToCartographer(
+    const sensor_msgs::MultiEchoLaserScan& msg) {
+  ::cartographer::sensor::proto::LaserScan proto;
+  proto.set_angle_min(msg.angle_min);
+  proto.set_angle_max(msg.angle_max);
+  proto.set_angle_increment(msg.angle_increment);
+  proto.set_time_increment(msg.time_increment);
+  proto.set_scan_time(msg.scan_time);
+  proto.set_range_min(msg.range_min);
+  proto.set_range_max(msg.range_max);
+
+  for (const auto& range : msg.ranges) {
+    auto* proto_echoes = proto.add_range()->mutable_value();
+    for (const auto& echo : range.echoes) {
+      proto_echoes->Add(echo);
+    }
+  }
+  for (const auto& intensity : msg.intensities) {
+    auto* proto_echoes = proto.add_intensity()->mutable_value();
+    for (const auto& echo : intensity.echoes) {
+      proto_echoes->Add(echo);
+    }
+  }
+  return proto;
+}
+
 ::cartographer::sensor::proto::LaserFan3D ToCartographer(
     const pcl::PointCloud<pcl::PointXYZ>& pcl_points) {
   ::cartographer::sensor::proto::LaserFan3D proto;

cartographer_ros/src/msg_conversion.h

@@ -49,6 +49,9 @@ sensor_msgs::PointCloud2 ToPointCloud2Message(
 ::cartographer::sensor::proto::LaserScan ToCartographer(
     const sensor_msgs::LaserScan& msg);
 
+::cartographer::sensor::proto::LaserScan ToCartographer(
+    const sensor_msgs::MultiEchoLaserScan& msg);
+
 ::cartographer::sensor::proto::LaserFan3D ToCartographer(
     const pcl::PointCloud<pcl::PointXYZ>& pcl_points);
 

cartographer_ros/src/submaps_display.cc

@@ -34,9 +34,9 @@
 #include <OgreTextureManager.h>
 #include <OgreViewport.h>
 #include <cartographer/common/mutex.h>
-#include <geometry_msgs/TransformStamped.h>
 #include <cartographer_ros_msgs/SubmapList.h>
 #include <cartographer_ros_msgs/SubmapQuery.h>
+#include <geometry_msgs/TransformStamped.h>
 #include <pluginlib/class_list_macros.h>
 #include <ros/package.h>
 #include <ros/ros.h>
@@ -45,6 +45,8 @@
 #include <rviz/properties/ros_topic_property.h>
 #include <rviz/properties/string_property.h>
 
+#include "node_constants.h"
+
 namespace cartographer_ros {
 namespace rviz {
 
@@ -55,8 +57,7 @@ constexpr char kMaterialsDirectory[] = "/ogre_media/materials";
 constexpr char kGlsl120Directory[] = "/glsl120";
 constexpr char kScriptsDirectory[] = "/scripts";
 constexpr char kScreenBlitMaterialName[] = "ScreenBlitMaterial";
-constexpr char kScreenBlitSourceMaterialName[] =
+constexpr char kScreenBlitSourceMaterialName[] = "cartographer_ros/ScreenBlit";
-    "cartographer_ros/ScreenBlit";
 constexpr char kSubmapsRttPrefix[] = "SubmapsRtt";
 constexpr char kMapTextureName[] = "MapTexture";
 constexpr char kMapOverlayName[] = "MapOverlay";
@@ -74,13 +75,15 @@ SubmapsDisplay::SubmapsDisplay()
       tf_listener_(tf_buffer_) {
   connect(this, SIGNAL(SubmapListUpdated()), this, SLOT(RequestNewSubmaps()));
   topic_property_ = new ::rviz::RosTopicProperty(
-      "Topic", "",
+      "Topic", QString("/cartographer/") + kSubmapListTopic,
-      QString::fromStdString(ros::message_traits::datatype<
+      QString::fromStdString(
-                             ::cartographer_ros_msgs::SubmapList>()),
+          ros::message_traits::datatype<::cartographer_ros_msgs::SubmapList>()),
       "cartographer_ros_msgs::SubmapList topic to subscribe to.", this,
       SLOT(UpdateTopic()));
   submap_query_service_property_ = new ::rviz::StringProperty(
-      "Submap query service", "", "Submap query service to connect to.", this,
+      "Submap query service",
+      QString("/cartographer/") + kSubmapQueryServiceName,
+      "Submap query service to connect to.", this,
       SLOT(UpdateSubmapQueryServiceName()));
   map_frame_property_ = new ::rviz::StringProperty(
       "Map frame", kDefaultMapFrame, "Map frame, used for fading out submaps.",
@@ -88,8 +91,7 @@ SubmapsDisplay::SubmapsDisplay()
   tracking_frame_property_ = new ::rviz::StringProperty(
       "Tracking frame", kDefaultTrackingFrame,
       "Tracking frame, used for fading out submaps.", this);
-  client_ =
+  client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>("");
-      update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>("");
   const std::string package_path = ::ros::package::getPath(ROS_PACKAGE_NAME);
   Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
       package_path + kMaterialsDirectory, "FileSystem", ROS_PACKAGE_NAME);
@@ -136,7 +138,8 @@ void SubmapsDisplay::onInitialize() {
       kSubmapTexturesGroup);
 
   scene_manager_->addListener(&scene_manager_listener_);
-  UpdateTopic();
+  // TODO(whess): Combine UpdateTopic()/UpdateSubmapQueryServiceName().
+  UpdateSubmapQueryServiceName();
 }
 
 void SubmapsDisplay::UpdateTopic() {

cartographer_ros/urdf/backpack_2d.urdf

@@ -25,7 +25,7 @@
     <color rgba="0.2 0.4 0.2 1"/>
   </material>
 
-  <link name="/imu_link">
+  <link name="imu_link">
     <visual>
       <origin xyz="0.01 0.01 -0.01"/>
       <geometry>
@@ -35,7 +35,7 @@
     </visual>
   </link>
 
-  <link name="/horizontal_laser_link">
+  <link name="horizontal_laser_link">
     <visual>
       <origin xyz="0.0 0.0 -0.01"/>
       <geometry>
@@ -45,7 +45,7 @@
     </visual>
   </link>
 
-  <link name="/vertical_laser_link">
+  <link name="vertical_laser_link">
     <visual>
       <origin xyz="0.0 0.0 -0.01"/>
       <geometry>
@@ -55,7 +55,7 @@
     </visual>
   </link>
 
-  <link name="/base_footprint">
+  <link name="base_link">
     <visual>
       <origin xyz="0.18 0.0 0.2"/>
       <geometry>
@@ -65,21 +65,21 @@
     </visual>
   </link>
 
-  <joint name="base_link_joint" type="fixed">
+  <joint name="imu_link_joint" type="fixed">
-    <child link="/imu_link"/>
+    <parent link="base_link"/>
-    <parent link="/base_footprint"/>
+    <child link="imu_link"/>
     <origin xyz="0 0 0" rpy="0 0 3.1416"/>
   </joint>
 
   <joint name="horizontal_laser_link_joint" type="fixed">
-    <parent link="/imu_link"/>
+    <parent link="base_link"/>
-    <child link="/horizontal_laser_link"/>
+    <child link="horizontal_laser_link"/>
     <origin xyz="0.1251 0.0937 0.05262"/>
   </joint>
 
   <joint name="vertical_laser_link_joint" type="fixed">
-    <parent link="/imu_link"/>
+    <parent link="base_link"/>
-    <child link="/vertical_laser_link"/>
+    <child link="vertical_laser_link"/>
     <origin rpy="0.0 -1.57 3.14" xyz="0.1251 0.0937 0.17772"/>
   </joint>
 </robot>