Cleanup. (#203)

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -136,8 +136,8 @@ MapBuilderBridge::GetTrajectoryStates() {
 
     const cartographer::mapping::TrajectoryBuilder* const trajectory_builder =
         map_builder_.GetTrajectoryBuilder(trajectory_id);
-    const cartographer::mapping::TrajectoryBuilder::PoseEstimate
+    const cartographer::mapping::TrajectoryBuilder::PoseEstimate pose_estimate =
-        pose_estimate = trajectory_builder->pose_estimate();
+        trajectory_builder->pose_estimate();
     if (cartographer::common::ToUniversal(pose_estimate.time) < 0) {
       continue;
     }

cartographer_ros/cartographer_ros/map_builder_bridge.h

@@ -18,8 +18,8 @@
 #define CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H_
 
 #include <memory>
-#include <unordered_set>
 #include <unordered_map>
+#include <unordered_set>
 
 #include "cartographer/mapping/map_builder.h"
 #include "cartographer_ros/node_options.h"

cartographer_ros/cartographer_ros/node.cc

@@ -48,8 +48,7 @@ using carto::transform::Rigid3d;
 constexpr int kLatestOnlyPublisherQueueSize = 1;
 
 Node::Node(const NodeOptions& options, tf2_ros::Buffer* const tf_buffer)
-    : options_(options),
+    : options_(options), map_builder_bridge_(options_, tf_buffer) {}
-      map_builder_bridge_(options_, tf_buffer) {}
 
 Node::~Node() {
   {
@@ -120,7 +119,8 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
 
     // We only publish a point cloud if it has changed. It is not needed at high
     // frequency, and republishing it would be computationally wasteful.
-    if (trajectory_state.pose_estimate.time != last_scan_matched_point_cloud_time_) {
+    if (trajectory_state.pose_estimate.time !=
+        last_scan_matched_point_cloud_time_) {
       scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
           carto::common::ToUniversal(trajectory_state.pose_estimate.time),
           options_.tracking_frame,
@@ -142,7 +142,8 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
         // TODO(damonkohler): 'odom_frame' and 'published_frame' must be
         // per-trajectory to fully support the multi-robot use case.
         stamped_transform.child_frame_id = options_.odom_frame;
-        stamped_transform.transform = ToGeometryMsgTransform(trajectory_state.local_to_map);
+        stamped_transform.transform =
+            ToGeometryMsgTransform(trajectory_state.local_to_map);
         stamped_transforms.push_back(stamped_transform);
 
         stamped_transform.header.frame_id = options_.odom_frame;

cartographer_ros/cartographer_ros/node_main.cc

@@ -143,14 +143,13 @@ void Run() {
           kFinishTrajectoryServiceName,
           boost::function<bool(
               ::cartographer_ros_msgs::FinishTrajectory::Request&,
-              ::cartographer_ros_msgs::FinishTrajectory::Response&)>(
+              ::cartographer_ros_msgs::FinishTrajectory::Response&)>([&](
-              [&](::cartographer_ros_msgs::FinishTrajectory::Request& request,
+              ::cartographer_ros_msgs::FinishTrajectory::Request& request,
               ::cartographer_ros_msgs::FinishTrajectory::Response&) {
             const int previous_trajectory_id = trajectory_id;
             trajectory_id = node.map_builder_bridge()->AddTrajectory(
                 expected_sensor_ids, options.tracking_frame);
-                node.map_builder_bridge()->FinishTrajectory(
+            node.map_builder_bridge()->FinishTrajectory(previous_trajectory_id);
-                    previous_trajectory_id);
             node.map_builder_bridge()->WriteAssets(request.stem);
             return true;
           }));

cartographer_ros/cartographer_ros/offline_node_main.cc

@@ -174,8 +174,7 @@ int main(int argc, char** argv) {
       << "-configuration_directory is missing.";
   CHECK(!FLAGS_configuration_basename.empty())
       << "-configuration_basename is missing.";
-  CHECK(!FLAGS_bag_filename.empty())
+  CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing.";
-      << "-bag_filename is missing.";
   CHECK(!FLAGS_urdf_filename.empty()) << "-urdf_filename is missing.";
 
   ::ros::init(argc, argv, "cartographer_offline_node");

cartographer_ros/cartographer_ros/time_conversion.cc

@@ -24,7 +24,9 @@ namespace cartographer_ros {
 ::ros::Time ToRos(::cartographer::common::Time time) {
   int64 uts_timestamp = ::cartographer::common::ToUniversal(time);
   int64 ns_since_unix_epoch =
-      (uts_timestamp - kUtsEpochOffsetFromUnixEpochInSeconds * 10000000ll) *
+      (uts_timestamp -
+       ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds *
+           10000000ll) *
       100ll;
   ::ros::Time ros_time;
   ros_time.fromNSec(ns_since_unix_epoch);
@@ -36,7 +38,9 @@ namespace cartographer_ros {
   // The epoch of the ICU Universal Time Scale is "0001-01-01 00:00:00.0 +0000",
   // exactly 719162 days before the Unix epoch.
   return ::cartographer::common::FromUniversal(
-      (time.sec + kUtsEpochOffsetFromUnixEpochInSeconds) * 10000000ll +
+      (time.sec +
+       ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds) *
+          10000000ll +
       (time.nsec + 50) / 100);  // + 50 to get the rounding correct.
 }
 

cartographer_ros/cartographer_ros/time_conversion.h

@@ -22,9 +22,6 @@
 
 namespace cartographer_ros {
 
-constexpr int64 kUtsEpochOffsetFromUnixEpochInSeconds =
-    (719162ll * 24ll * 60ll * 60ll);
-
 ::ros::Time ToRos(::cartographer::common::Time time);
 
 ::cartographer::common::Time FromRos(const ::ros::Time& time);

cartographer_ros/cartographer_ros/time_conversion_test.cc

@@ -34,7 +34,8 @@ TEST(TimeConversion, testToRos) {
     ros_now.fromSec(seconds_since_epoch);
     ::cartographer::common::Time cartographer_now(
         ::cartographer::common::FromSeconds(
-            seconds_since_epoch + kUtsEpochOffsetFromUnixEpochInSeconds));
+            seconds_since_epoch +
+            ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds));
     EXPECT_EQ(cartographer_now, ::cartographer_ros::FromRos(ros_now));
     EXPECT_EQ(ros_now, ::cartographer_ros::ToRos(cartographer_now));
   }

cartographer_ros/urdf/backpack_2d.urdf

@@ -21,9 +21,6 @@
   <material name="gray">
     <color rgba="0.2 0.2 0.2 1" />
   </material>
-  <material name="green">
-    <color rgba="0.2 0.4 0.2 1" />
-  </material>
 
   <link name="imu_link">
     <visual>

cartographer_ros/urdf/backpack_3d.urdf

@@ -21,9 +21,6 @@
   <material name="gray">
     <color rgba="0.2 0.2 0.2 1" />
   </material>
-  <material name="green">
-    <color rgba="0.2 0.4 0.2 1" />
-  </material>
 
   <link name="imu_link">
     <visual>