From 78c14d50d23ddc4c90a09acff7b7050cafb52c1d Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 30 Nov 2016 16:15:43 +0100 Subject: [PATCH] Cleanup. (#203) --- .../cartographer_ros/map_builder_bridge.cc | 4 ++-- .../cartographer_ros/map_builder_bridge.h | 2 +- cartographer_ros/cartographer_ros/node.cc | 9 ++++---- .../cartographer_ros/node_main.cc | 21 +++++++++---------- .../cartographer_ros/offline_node_main.cc | 3 +-- .../cartographer_ros/time_conversion.cc | 8 +++++-- .../cartographer_ros/time_conversion.h | 3 --- .../cartographer_ros/time_conversion_test.cc | 3 ++- cartographer_ros/urdf/backpack_2d.urdf | 3 --- cartographer_ros/urdf/backpack_3d.urdf | 3 --- 10 files changed, 27 insertions(+), 32 deletions(-) diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 359aa34..18e7c2f 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/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 - pose_estimate = trajectory_builder->pose_estimate(); + const cartographer::mapping::TrajectoryBuilder::PoseEstimate pose_estimate = + trajectory_builder->pose_estimate(); if (cartographer::common::ToUniversal(pose_estimate.time) < 0) { continue; } diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index 901b175..232fc73 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -18,8 +18,8 @@ #define CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H_ #include -#include #include +#include #include "cartographer/mapping/map_builder.h" #include "cartographer_ros/node_options.h" diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 01925ac..44e49e2 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/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), - map_builder_bridge_(options_, tf_buffer) {} + : options_(options), 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; diff --git a/cartographer_ros/cartographer_ros/node_main.cc b/cartographer_ros/cartographer_ros/node_main.cc index 5dda6d3..aea0452 100644 --- a/cartographer_ros/cartographer_ros/node_main.cc +++ b/cartographer_ros/cartographer_ros/node_main.cc @@ -143,17 +143,16 @@ void Run() { kFinishTrajectoryServiceName, boost::function( - [&](::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( - previous_trajectory_id); - node.map_builder_bridge()->WriteAssets(request.stem); - return true; - })); + ::cartographer_ros_msgs::FinishTrajectory::Response&)>([&]( + ::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(previous_trajectory_id); + node.map_builder_bridge()->WriteAssets(request.stem); + return true; + })); ::ros::spin(); diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index 65d7f58..97b0aa3 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/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()) - << "-bag_filename is missing."; + CHECK(!FLAGS_bag_filename.empty()) << "-bag_filename is missing."; CHECK(!FLAGS_urdf_filename.empty()) << "-urdf_filename is missing."; ::ros::init(argc, argv, "cartographer_offline_node"); diff --git a/cartographer_ros/cartographer_ros/time_conversion.cc b/cartographer_ros/cartographer_ros/time_conversion.cc index eb5e815..6371c57 100644 --- a/cartographer_ros/cartographer_ros/time_conversion.cc +++ b/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. } diff --git a/cartographer_ros/cartographer_ros/time_conversion.h b/cartographer_ros/cartographer_ros/time_conversion.h index 863d871..eaf3115 100644 --- a/cartographer_ros/cartographer_ros/time_conversion.h +++ b/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); diff --git a/cartographer_ros/cartographer_ros/time_conversion_test.cc b/cartographer_ros/cartographer_ros/time_conversion_test.cc index c7e110a..4443ca4 100644 --- a/cartographer_ros/cartographer_ros/time_conversion_test.cc +++ b/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)); } diff --git a/cartographer_ros/urdf/backpack_2d.urdf b/cartographer_ros/urdf/backpack_2d.urdf index 478a311..c9c8f97 100644 --- a/cartographer_ros/urdf/backpack_2d.urdf +++ b/cartographer_ros/urdf/backpack_2d.urdf @@ -21,9 +21,6 @@ - - - diff --git a/cartographer_ros/urdf/backpack_3d.urdf b/cartographer_ros/urdf/backpack_3d.urdf index 0d31d0d..0d71046 100644 --- a/cartographer_ros/urdf/backpack_3d.urdf +++ b/cartographer_ros/urdf/backpack_3d.urdf @@ -21,9 +21,6 @@ - - -