From ff92e9e4bda2edb052c6cc0e1bb3e94b370876bb Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Wed, 8 Nov 2017 10:37:02 +0100 Subject: [PATCH] Follow googlecartographer/cartographer#637. (#583) Use c++ standard types instead of the values from port.h. --- .../cartographer_ros/assets_writer_main.cc | 29 +++++++------ .../cartographer_ros/map_builder_bridge.cc | 4 +- .../cartographer_ros/map_builder_bridge.h | 4 +- .../cartographer_ros/msg_conversion.cc | 6 +-- .../cartographer_ros/msg_conversion.h | 2 +- cartographer_ros/cartographer_ros/node.cc | 43 ++++++++++--------- cartographer_ros/cartographer_ros/node.h | 23 +++++----- .../cartographer_ros/node_options.cc | 8 ++-- .../cartographer_ros/node_options.h | 6 +-- .../occupancy_grid_node_main.cc | 11 ++--- .../cartographer_ros/offline_node_main.cc | 20 ++++----- .../ros_map_writing_points_processor.cc | 5 ++- .../ros_map_writing_points_processor.h | 4 +- .../cartographer_ros/rosbag_validate_main.cc | 41 +++++++++--------- .../cartographer_ros/sensor_bridge.cc | 24 ++++++----- .../cartographer_ros/sensor_bridge.h | 20 ++++----- .../cartographer_ros/start_trajectory_main.cc | 4 +- .../cartographer_ros/tf_bridge.cc | 5 ++- cartographer_ros/cartographer_ros/tf_bridge.h | 8 ++-- .../cartographer_ros/time_conversion.cc | 4 +- .../cartographer_ros/time_conversion_test.cc | 4 +- .../cartographer_ros/trajectory_options.h | 6 +-- .../cartographer_ros/urdf_reader.cc | 2 +- .../cartographer_ros/urdf_reader.h | 2 +- 24 files changed, 147 insertions(+), 138 deletions(-) diff --git a/cartographer_ros/cartographer_ros/assets_writer_main.cc b/cartographer_ros/cartographer_ros/assets_writer_main.cc index 397c31a..12a2694 100644 --- a/cartographer_ros/cartographer_ros/assets_writer_main.cc +++ b/cartographer_ros/cartographer_ros/assets_writer_main.cc @@ -77,7 +77,7 @@ namespace carto = ::cartographer; template std::unique_ptr HandleMessage( - const T& message, const string& tracking_frame, + const T& message, const std::string& tracking_frame, const tf2_ros::Buffer& tf_buffer, const carto::transform::TransformInterpolationBuffer& transform_interpolation_buffer) { @@ -116,15 +116,16 @@ std::unique_ptr HandleMessage( return points_batch; } -void Run(const string& pose_graph_filename, - const std::vector& bag_filenames, - const string& configuration_directory, - const string& configuration_basename, const string& urdf_filename, - const string& output_file_prefix) { +void Run(const std::string& pose_graph_filename, + const std::vector& bag_filenames, + const std::string& configuration_directory, + const std::string& configuration_basename, + const std::string& urdf_filename, + const std::string& output_file_prefix) { auto file_resolver = carto::common::make_unique( - std::vector{configuration_directory}); - const string code = + std::vector{configuration_directory}); + const std::string code = file_resolver->GetFileContentOrDie(configuration_basename); carto::common::LuaParameterDictionary lua_parameter_dictionary( code, std::move(file_resolver)); @@ -139,10 +140,10 @@ void Run(const string& pose_graph_filename, "trajectory in the same order as the correponding trajectories in the " "pose graph proto."; - const string file_prefix = !output_file_prefix.empty() - ? output_file_prefix - : bag_filenames.front() + "_"; - const auto file_writer_factory = [file_prefix](const string& filename) { + const std::string file_prefix = !output_file_prefix.empty() + ? output_file_prefix + : bag_filenames.front() + "_"; + const auto file_writer_factory = [file_prefix](const std::string& filename) { return carto::common::make_unique(file_prefix + filename); }; @@ -169,14 +170,14 @@ void Run(const string& pose_graph_filename, builder.CreatePipeline( lua_parameter_dictionary.GetDictionary("pipeline").get()); - const string tracking_frame = + const std::string tracking_frame = lua_parameter_dictionary.GetString("tracking_frame"); do { for (size_t trajectory_id = 0; trajectory_id < bag_filenames.size(); ++trajectory_id) { const carto::mapping::proto::Trajectory& trajectory_proto = pose_graph_proto.trajectory(trajectory_id); - const string& bag_filename = bag_filenames[trajectory_id]; + const std::string& bag_filename = bag_filenames[trajectory_id]; LOG(INFO) << "Processing " << bag_filename << "..."; if (trajectory_proto.node_size() == 0) { continue; diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 3fc7aa5..80e8287 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -75,7 +75,7 @@ void MapBuilderBridge::LoadMap(const std::string& map_filename) { } int MapBuilderBridge::AddTrajectory( - const std::unordered_set& expected_sensor_ids, + const std::unordered_set& expected_sensor_ids, const TrajectoryOptions& trajectory_options) { const int trajectory_id = map_builder_.AddTrajectoryBuilder( expected_sensor_ids, trajectory_options.trajectory_builder_options); @@ -150,7 +150,7 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() { cartographer_ros_msgs::SubmapList submap_list; submap_list.header.stamp = ::ros::Time::now(); submap_list.header.frame_id = node_options_.map_frame; - for (const auto &submap_id_data : + for (const auto& submap_id_data : map_builder_.sparse_pose_graph()->GetAllSubmapData()) { cartographer_ros_msgs::SubmapEntry submap_entry; submap_entry.trajectory_id = submap_id_data.id.trajectory_id; diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.h b/cartographer_ros/cartographer_ros/map_builder_bridge.h index 7cc6c19..f4d4feb 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.h +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.h @@ -51,11 +51,11 @@ class MapBuilderBridge { MapBuilderBridge& operator=(const MapBuilderBridge&) = delete; void LoadMap(const std::string& map_filename); - int AddTrajectory(const std::unordered_set& expected_sensor_ids, + int AddTrajectory(const std::unordered_set& expected_sensor_ids, const TrajectoryOptions& trajectory_options); void FinishTrajectory(int trajectory_id); void RunFinalOptimization(); - void SerializeState(const string& filename); + void SerializeState(const std::string& filename); bool HandleSubmapQuery( cartographer_ros_msgs::SubmapQuery::Request& request, diff --git a/cartographer_ros/cartographer_ros/msg_conversion.cc b/cartographer_ros/cartographer_ros/msg_conversion.cc index 4cd2abf..5da7098 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.cc +++ b/cartographer_ros/cartographer_ros/msg_conversion.cc @@ -46,8 +46,8 @@ constexpr float kPointCloudComponentFourMagic = 1.; using ::cartographer::sensor::PointCloudWithIntensities; using ::cartographer::transform::Rigid3d; -sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64 timestamp, - const string& frame_id, +sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64_t timestamp, + const std::string& frame_id, const int num_points) { sensor_msgs::PointCloud2 msg; msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp)); @@ -140,7 +140,7 @@ bool PointCloud2HasField(const sensor_msgs::PointCloud2& pc2, } // namespace sensor_msgs::PointCloud2 ToPointCloud2Message( - const int64 timestamp, const string& frame_id, + const int64_t timestamp, const std::string& frame_id, 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()); diff --git a/cartographer_ros/cartographer_ros/msg_conversion.h b/cartographer_ros/cartographer_ros/msg_conversion.h index eea4e21..ee2f576 100644 --- a/cartographer_ros/cartographer_ros/msg_conversion.h +++ b/cartographer_ros/cartographer_ros/msg_conversion.h @@ -34,7 +34,7 @@ namespace cartographer_ros { sensor_msgs::PointCloud2 ToPointCloud2Message( - int64 timestamp, const string& frame_id, + int64_t timestamp, const std::string& frame_id, const ::cartographer::sensor::TimedPointCloud& point_cloud); geometry_msgs::Transform ToGeometryMsgTransform( diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index f986a04..60cde5a 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -60,9 +60,9 @@ cartographer_ros_msgs::SensorTopics DefaultSensorTopics() { // calls 'handler' on the 'node' to handle messages. Returns the subscriber. template ::ros::Subscriber SubscribeWithHandler( - void (Node::*handler)(int, const string&, + void (Node::*handler)(int, const std::string&, const typename MessageType::ConstPtr&), - const int trajectory_id, const string& topic, + const int trajectory_id, const std::string& topic, ::ros::NodeHandle* const node_handle, Node* const node) { return node_handle->subscribe( topic, kInfiniteSubscriberQueueSize, @@ -250,21 +250,21 @@ void Node::PublishConstraintList( } } -std::unordered_set Node::ComputeExpectedTopics( +std::unordered_set Node::ComputeExpectedTopics( const TrajectoryOptions& options, const cartographer_ros_msgs::SensorTopics& topics) { - std::unordered_set expected_topics; + std::unordered_set expected_topics; // Subscribe to all laser scan, multi echo laser scan, and point cloud topics. - for (const string& topic : ComputeRepeatedTopicNames( + for (const std::string& topic : ComputeRepeatedTopicNames( topics.laser_scan_topic, options.num_laser_scans)) { expected_topics.insert(topic); } - for (const string& topic : + for (const std::string& topic : ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic, options.num_multi_echo_laser_scans)) { expected_topics.insert(topic); } - for (const string& topic : ComputeRepeatedTopicNames( + for (const std::string& topic : ComputeRepeatedTopicNames( topics.point_cloud2_topic, options.num_point_clouds)) { expected_topics.insert(topic); } @@ -285,7 +285,7 @@ std::unordered_set Node::ComputeExpectedTopics( int Node::AddTrajectory(const TrajectoryOptions& options, const cartographer_ros_msgs::SensorTopics& topics) { - const std::unordered_set expected_sensor_ids = + const std::unordered_set expected_sensor_ids = ComputeExpectedTopics(options, topics); const int trajectory_id = map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); @@ -301,7 +301,7 @@ int Node::AddTrajectory(const TrajectoryOptions& options, void Node::LaunchSubscribers(const TrajectoryOptions& options, const cartographer_ros_msgs::SensorTopics& topics, const int trajectory_id) { - for (const string& topic : ComputeRepeatedTopicNames( + for (const std::string& topic : ComputeRepeatedTopicNames( topics.laser_scan_topic, options.num_laser_scans)) { subscribers_[trajectory_id].push_back( {SubscribeWithHandler( @@ -309,7 +309,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, this), topic}); } - for (const string& topic : + for (const std::string& topic : ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic, options.num_multi_echo_laser_scans)) { subscribers_[trajectory_id].push_back( @@ -318,7 +318,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, &node_handle_, this), topic}); } - for (const string& topic : ComputeRepeatedTopicNames( + for (const std::string& topic : ComputeRepeatedTopicNames( topics.point_cloud2_topic, options.num_point_clouds)) { subscribers_[trajectory_id].push_back( {SubscribeWithHandler( @@ -333,7 +333,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, (node_options_.map_builder_options.use_trajectory_builder_2d() && options.trajectory_builder_options.trajectory_builder_2d_options() .use_imu_data())) { - string topic = topics.imu_topic; + std::string topic = topics.imu_topic; subscribers_[trajectory_id].push_back( {SubscribeWithHandler(&Node::HandleImuMessage, trajectory_id, topic, @@ -342,7 +342,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, } if (options.use_odometry) { - string topic = topics.odometry_topic; + std::string topic = topics.odometry_topic; subscribers_[trajectory_id].push_back( {SubscribeWithHandler(&Node::HandleOdometryMessage, trajectory_id, topic, @@ -399,14 +399,14 @@ void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) { AddTrajectory(options, DefaultSensorTopics()); } -std::unordered_set Node::ComputeDefaultTopics( +std::unordered_set Node::ComputeDefaultTopics( const TrajectoryOptions& options) { carto::common::MutexLocker lock(&mutex_); return ComputeExpectedTopics(options, DefaultSensorTopics()); } int Node::AddOfflineTrajectory( - const std::unordered_set& expected_sensor_ids, + const std::unordered_set& expected_sensor_ids, const TrajectoryOptions& options) { carto::common::MutexLocker lock(&mutex_); const int trajectory_id = @@ -483,7 +483,7 @@ void Node::RunFinalOptimization() { } void Node::HandleOdometryMessage(const int trajectory_id, - const string& sensor_id, + const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) { carto::common::MutexLocker lock(&mutex_); if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) { @@ -497,7 +497,8 @@ void Node::HandleOdometryMessage(const int trajectory_id, sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg); } -void Node::HandleImuMessage(const int trajectory_id, const string& sensor_id, +void Node::HandleImuMessage(const int trajectory_id, + const std::string& sensor_id, const sensor_msgs::Imu::ConstPtr& msg) { carto::common::MutexLocker lock(&mutex_); if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) { @@ -512,7 +513,7 @@ void Node::HandleImuMessage(const int trajectory_id, const string& sensor_id, } void Node::HandleLaserScanMessage(const int trajectory_id, - const string& sensor_id, + const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) { carto::common::MutexLocker lock(&mutex_); if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { @@ -523,7 +524,7 @@ void Node::HandleLaserScanMessage(const int trajectory_id, } void Node::HandleMultiEchoLaserScanMessage( - int trajectory_id, const string& sensor_id, + int trajectory_id, const std::string& sensor_id, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { carto::common::MutexLocker lock(&mutex_); if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { @@ -534,7 +535,7 @@ void Node::HandleMultiEchoLaserScanMessage( } void Node::HandlePointCloud2Message( - const int trajectory_id, const string& sensor_id, + const int trajectory_id, const std::string& sensor_id, const sensor_msgs::PointCloud2::ConstPtr& msg) { carto::common::MutexLocker lock(&mutex_); if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { @@ -544,7 +545,7 @@ void Node::HandlePointCloud2Message( ->HandlePointCloud2Message(sensor_id, msg); } -void Node::SerializeState(const string& filename) { +void Node::SerializeState(const std::string& filename) { carto::common::MutexLocker lock(&mutex_); map_builder_bridge_.SerializeState(filename); } diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index b50bbbe..06b1c88 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -64,29 +64,29 @@ class Node { void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options); // Compute the default topics for the given 'options'. - std::unordered_set ComputeDefaultTopics( + std::unordered_set ComputeDefaultTopics( const TrajectoryOptions& options); // Adds a trajectory for offline processing, i.e. not listening to topics. int AddOfflineTrajectory( - const std::unordered_set& expected_sensor_ids, + const std::unordered_set& expected_sensor_ids, const TrajectoryOptions& options); // The following functions handle adding sensor data to a trajectory. - void HandleOdometryMessage(int trajectory_id, const string& sensor_id, + void HandleOdometryMessage(int trajectory_id, const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg); - void HandleImuMessage(int trajectory_id, const string& sensor_id, + void HandleImuMessage(int trajectory_id, const std::string& sensor_id, const sensor_msgs::Imu::ConstPtr& msg); - void HandleLaserScanMessage(int trajectory_id, const string& sensor_id, + void HandleLaserScanMessage(int trajectory_id, const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg); void HandleMultiEchoLaserScanMessage( - int trajectory_id, const string& sensor_id, + int trajectory_id, const std::string& sensor_id, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg); - void HandlePointCloud2Message(int trajectory_id, const string& sensor_id, + void HandlePointCloud2Message(int trajectory_id, const std::string& sensor_id, const sensor_msgs::PointCloud2::ConstPtr& msg); // Serializes the complete Node state. - void SerializeState(const string& filename); + void SerializeState(const std::string& filename); // Loads a persisted state to use as a map. void LoadMap(const std::string& map_filename); @@ -97,10 +97,11 @@ class Node { struct Subscriber { ::ros::Subscriber subscriber; - // ::ros::Subscriber::getTopic() does not necessarily return the same string + // ::ros::Subscriber::getTopic() does not necessarily return the same + // std::string // it was given in its constructor. Since we rely on the topic name as the // unique identifier of a subscriber, we remember it ourselves. - string topic; + std::string topic; }; bool HandleSubmapQuery( @@ -115,7 +116,7 @@ class Node { bool HandleWriteState(cartographer_ros_msgs::WriteState::Request& request, cartographer_ros_msgs::WriteState::Response& response); // Returns the set of topic names we want to subscribe to. - std::unordered_set ComputeExpectedTopics( + std::unordered_set ComputeExpectedTopics( const TrajectoryOptions& options, const cartographer_ros_msgs::SensorTopics& topics); int AddTrajectory(const TrajectoryOptions& options, diff --git a/cartographer_ros/cartographer_ros/node_options.cc b/cartographer_ros/cartographer_ros/node_options.cc index ae87cb2..004006c 100644 --- a/cartographer_ros/cartographer_ros/node_options.cc +++ b/cartographer_ros/cartographer_ros/node_options.cc @@ -44,12 +44,12 @@ NodeOptions CreateNodeOptions( } std::tuple LoadOptions( - const string& configuration_directory, - const string& configuration_basename) { + const std::string& configuration_directory, + const std::string& configuration_basename) { auto file_resolver = cartographer::common::make_unique< cartographer::common::ConfigurationFileResolver>( - std::vector{configuration_directory}); - const string code = + std::vector{configuration_directory}); + const std::string code = file_resolver->GetFileContentOrDie(configuration_basename); cartographer::common::LuaParameterDictionary lua_parameter_dictionary( code, std::move(file_resolver)); diff --git a/cartographer_ros/cartographer_ros/node_options.h b/cartographer_ros/cartographer_ros/node_options.h index 1687362..cd0f997 100644 --- a/cartographer_ros/cartographer_ros/node_options.h +++ b/cartographer_ros/cartographer_ros/node_options.h @@ -30,7 +30,7 @@ namespace cartographer_ros { // Top-level options of Cartographer's ROS integration. struct NodeOptions { ::cartographer::mapping::proto::MapBuilderOptions map_builder_options; - string map_frame; + std::string map_frame; double lookup_transform_timeout_sec; double submap_publish_period_sec; double pose_publish_period_sec; @@ -41,8 +41,8 @@ NodeOptions CreateNodeOptions( ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary); std::tuple LoadOptions( - const string& configuration_directory, - const string& configuration_basename); + const std::string& configuration_directory, + const std::string& configuration_basename); } // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc index 30c070d..8e0d9a1 100644 --- a/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc +++ b/cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc @@ -107,7 +107,7 @@ class Node { private: void HandleSubmapList(const cartographer_ros_msgs::SubmapList::ConstPtr& msg); void DrawAndPublish(const ::ros::WallTimerEvent& timer_event); - void PublishOccupancyGrid(const string& frame_id, const ros::Time& time, + void PublishOccupancyGrid(const std::string& frame_id, const ros::Time& time, const Eigen::Array2f& origin, const Eigen::Array2i& size, cairo_surface_t* surface); @@ -261,7 +261,8 @@ void Node::DrawAndPublish(const ::ros::WallTimerEvent& unused_timer_event) { } } -void Node::PublishOccupancyGrid(const string& frame_id, const ros::Time& time, +void Node::PublishOccupancyGrid(const std::string& frame_id, + const ros::Time& time, const Eigen::Array2f& origin, const Eigen::Array2i& size, cairo_surface_t* surface) { @@ -281,12 +282,12 @@ void Node::PublishOccupancyGrid(const string& frame_id, const ros::Time& time, occupancy_grid.info.origin.orientation.y = 0.; occupancy_grid.info.origin.orientation.z = 0.; - const uint32* pixel_data = - reinterpret_cast(cairo_image_surface_get_data(surface)); + const uint32_t* pixel_data = + reinterpret_cast(cairo_image_surface_get_data(surface)); occupancy_grid.data.reserve(size.x() * size.y()); for (int y = size.y() - 1; y >= 0; --y) { for (int x = 0; x < size.x(); ++x) { - const uint32 packed = pixel_data[y * size.x() + x]; + const uint32_t packed = pixel_data[y * size.x() + x]; const unsigned char color = packed >> 16; const unsigned char observed = packed >> 8; const int value = diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index 3b6d63c..d9668c7 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -16,8 +16,8 @@ #include #include -#include #include +#include #include #include #include @@ -68,7 +68,7 @@ constexpr char kTfTopic[] = "tf"; constexpr double kClockPublishFrequencySec = 1. / 30.; constexpr int kSingleThreaded = 1; -void Run(const std::vector& bag_filenames) { +void Run(const std::vector& bag_filenames) { const std::chrono::time_point start_time = std::chrono::steady_clock::now(); NodeOptions node_options; @@ -97,8 +97,9 @@ void Run(const std::vector& bag_filenames) { node.LoadMap(FLAGS_pbstream_filename); } - std::unordered_set expected_sensor_ids; - for (const string& topic : node.ComputeDefaultTopics(trajectory_options)) { + std::unordered_set expected_sensor_ids; + for (const std::string& topic : + node.ComputeDefaultTopics(trajectory_options)) { CHECK(expected_sensor_ids.insert(node.node_handle()->resolveName(topic)) .second); } @@ -127,7 +128,7 @@ void Run(const std::vector& bag_filenames) { }, false /* oneshot */, false /* autostart */); - for (const string& bag_filename : bag_filenames) { + for (const std::string& bag_filename : bag_filenames) { if (!::ros::ok()) { break; } @@ -169,10 +170,9 @@ void Run(const std::vector& bag_filenames) { } while (!delayed_messages.empty() && - delayed_messages.front().getTime() < - msg.getTime() - kDelay) { + delayed_messages.front().getTime() < msg.getTime() - kDelay) { const rosbag::MessageInstance& delayed_msg = delayed_messages.front(); - const string topic = node.node_handle()->resolveName( + const std::string topic = node.node_handle()->resolveName( delayed_msg.getTopic(), false /* resolve */); if (delayed_msg.isType()) { node.HandleLaserScanMessage( @@ -208,7 +208,7 @@ void Run(const std::vector& bag_filenames) { delayed_messages.pop_front(); } - const string topic = + const std::string topic = node.node_handle()->resolveName(msg.getTopic(), false /* resolve */); if (expected_sensor_ids.count(topic) == 0) { continue; @@ -245,7 +245,7 @@ void Run(const std::vector& bag_filenames) { #endif if (::ros::ok()) { - const string output_filename = bag_filenames.front() + ".pbstream"; + const std::string output_filename = bag_filenames.front() + ".pbstream"; LOG(INFO) << "Writing state to '" << output_filename << "'..."; node.SerializeState(output_filename); } diff --git a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc index c4f8770..a2c762f 100644 --- a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc +++ b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc @@ -42,7 +42,7 @@ void WritePgm(const ::cartographer::io::Image& image, const double resolution, void WriteYaml(const ::cartographer::io::Image& image, const ::cartographer::mapping_2d::MapLimits& limits, - const string& pgm_filename, const Eigen::Array2i& offset, + const std::string& pgm_filename, const Eigen::Array2i& offset, ::cartographer::io::FileWriter* file_writer) { const double resolution = limits.resolution(); const double x_offset = @@ -66,7 +66,8 @@ RosMapWritingPointsProcessor::RosMapWritingPointsProcessor( const ::cartographer::mapping_2d::proto::RangeDataInserterOptions& range_data_inserter_options, ::cartographer::io::FileWriterFactory file_writer_factory, - const string& filestem, ::cartographer::io::PointsProcessor* const next) + const std::string& filestem, + ::cartographer::io::PointsProcessor* const next) : filestem_(filestem), next_(next), file_writer_factory_(file_writer_factory), diff --git a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h index 1dabc0c..df64724 100644 --- a/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h +++ b/cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h @@ -36,7 +36,7 @@ class RosMapWritingPointsProcessor const ::cartographer::mapping_2d::proto::RangeDataInserterOptions& range_data_inserter_options, ::cartographer::io::FileWriterFactory file_writer_factory, - const string& filestem, PointsProcessor* next); + const std::string& filestem, PointsProcessor* next); RosMapWritingPointsProcessor(const RosMapWritingPointsProcessor&) = delete; RosMapWritingPointsProcessor& operator=(const RosMapWritingPointsProcessor&) = delete; @@ -52,7 +52,7 @@ class RosMapWritingPointsProcessor FlushResult Flush() override; private: - const string filestem_; + const std::string filestem_; PointsProcessor* const next_; ::cartographer::io::FileWriterFactory file_writer_factory_; ::cartographer::mapping_2d::RangeDataInserter range_data_inserter_; diff --git a/cartographer_ros/cartographer_ros/rosbag_validate_main.cc b/cartographer_ros/cartographer_ros/rosbag_validate_main.cc index cc487c6..a6c1d56 100644 --- a/cartographer_ros/cartographer_ros/rosbag_validate_main.cc +++ b/cartographer_ros/cartographer_ros/rosbag_validate_main.cc @@ -47,44 +47,45 @@ namespace { struct PerFrameId { ros::Time last_timestamp; - string topic; + std::string topic; ::cartographer::common::Histogram histogram; std::unique_ptr timing_file; }; -std::unique_ptr CreateTimingFile(const string& frame_id) { +std::unique_ptr CreateTimingFile(const std::string& frame_id) { auto timing_file = ::cartographer::common::make_unique( std::string("timing_") + frame_id + ".csv", std::ios_base::out); - (*timing_file) << "# Timing information for sensor with frame id: " - << frame_id << std::endl - << "# Columns are in order" << std::endl - << "# - packet index of the packet in the bag, first packet is 1" - << std::endl - << "# - timestamp when rosbag wrote the packet, i.e. " - "rosbag::MessageInstance::getTime().toNSec()" - << std::endl - << "# - timestamp when data was acquired, i.e. " - "message.header.stamp.toNSec()" - << std::endl - << "#" << std::endl - << "# The data can be read in python using" << std::endl - << "# import numpy" << std::endl - << "# np.loadtxt(, dtype='uint64')" << std::endl; + (*timing_file) + << "# Timing information for sensor with frame id: " << frame_id + << std::endl + << "# Columns are in order" << std::endl + << "# - packet index of the packet in the bag, first packet is 1" + << std::endl + << "# - timestamp when rosbag wrote the packet, i.e. " + "rosbag::MessageInstance::getTime().toNSec()" + << std::endl + << "# - timestamp when data was acquired, i.e. " + "message.header.stamp.toNSec()" + << std::endl + << "#" << std::endl + << "# The data can be read in python using" << std::endl + << "# import numpy" << std::endl + << "# np.loadtxt(, dtype='uint64')" << std::endl; return timing_file; } -void Run(const string& bag_filename, const bool dump_timing) { +void Run(const std::string& bag_filename, const bool dump_timing) { rosbag::Bag bag; bag.open(bag_filename, rosbag::bagmode::Read); rosbag::View view(bag); - std::map per_frame_id; + std::map per_frame_id; size_t message_index = 0; for (const rosbag::MessageInstance& message : view) { ++message_index; - string frame_id; + std::string frame_id; ros::Time time; if (message.isType()) { auto msg = message.instantiate(); diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index 809d961..7f320d1 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/cartographer_ros/cartographer_ros/sensor_bridge.cc @@ -28,7 +28,7 @@ using carto::transform::Rigid3d; namespace { -const string& CheckNoLeadingSlash(const string& frame_id) { +const std::string& CheckNoLeadingSlash(const std::string& frame_id) { if (frame_id.size() > 0) { CHECK_NE(frame_id[0], '/') << "The frame_id " << frame_id << " should not start with a /. See 1.7 in " @@ -40,7 +40,8 @@ const string& CheckNoLeadingSlash(const string& frame_id) { } // namespace SensorBridge::SensorBridge( - const int num_subdivisions_per_laser_scan, const string& tracking_frame, + const int num_subdivisions_per_laser_scan, + const std::string& tracking_frame, const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer, carto::mapping::TrajectoryBuilder* const trajectory_builder) : num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan), @@ -62,7 +63,7 @@ SensorBridge::ToOdometryData(const nav_msgs::Odometry::ConstPtr& msg) { } void SensorBridge::HandleOdometryMessage( - const string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) { + const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) { std::unique_ptr<::cartographer::sensor::OdometryData> odometry_data = ToOdometryData(msg); if (odometry_data != nullptr) { @@ -101,7 +102,7 @@ std::unique_ptr<::cartographer::sensor::ImuData> SensorBridge::ToImuData( sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)}); } -void SensorBridge::HandleImuMessage(const string& sensor_id, +void SensorBridge::HandleImuMessage(const std::string& sensor_id, const sensor_msgs::Imu::ConstPtr& msg) { std::unique_ptr<::cartographer::sensor::ImuData> imu_data = ToImuData(msg); if (imu_data != nullptr) { @@ -112,20 +113,21 @@ void SensorBridge::HandleImuMessage(const string& sensor_id, } void SensorBridge::HandleLaserScanMessage( - const string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) { + const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) { HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id, ToPointCloudWithIntensities(*msg)); } void SensorBridge::HandleMultiEchoLaserScanMessage( - const string& sensor_id, + const std::string& sensor_id, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id, ToPointCloudWithIntensities(*msg)); } void SensorBridge::HandlePointCloud2Message( - const string& sensor_id, const sensor_msgs::PointCloud2::ConstPtr& msg) { + const std::string& sensor_id, + const sensor_msgs::PointCloud2::ConstPtr& msg) { pcl::PointCloud pcl_point_cloud; pcl::fromROSMsg(*msg, pcl_point_cloud); carto::sensor::TimedPointCloud point_cloud; @@ -139,8 +141,8 @@ void SensorBridge::HandlePointCloud2Message( const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; } void SensorBridge::HandleLaserScan( - const string& sensor_id, const carto::common::Time start_time, - const string& frame_id, + const std::string& sensor_id, const carto::common::Time start_time, + const std::string& frame_id, const carto::sensor::PointCloudWithIntensities& points) { // TODO(gaschler): Use per-point time instead of subdivisions. for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) { @@ -162,8 +164,8 @@ void SensorBridge::HandleLaserScan( } void SensorBridge::HandleRangefinder( - const string& sensor_id, const carto::common::Time time, - const string& frame_id, const carto::sensor::TimedPointCloud& ranges) { + const std::string& sensor_id, const carto::common::Time time, + const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) { const auto sensor_to_tracking = tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id)); if (sensor_to_tracking != nullptr) { diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.h b/cartographer_ros/cartographer_ros/sensor_bridge.h index f9d8d31..a6a0519 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.h +++ b/cartographer_ros/cartographer_ros/sensor_bridge.h @@ -40,7 +40,7 @@ namespace cartographer_ros { class SensorBridge { public: explicit SensorBridge( - int num_subdivisions_per_laser_scan, const string& tracking_frame, + int num_subdivisions_per_laser_scan, const std::string& tracking_frame, double lookup_transform_timeout_sec, tf2_ros::Buffer* tf_buffer, ::cartographer::mapping::TrajectoryBuilder* trajectory_builder); @@ -49,30 +49,30 @@ class SensorBridge { std::unique_ptr<::cartographer::sensor::OdometryData> ToOdometryData( const nav_msgs::Odometry::ConstPtr& msg); - void HandleOdometryMessage(const string& sensor_id, + void HandleOdometryMessage(const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg); std::unique_ptr<::cartographer::sensor::ImuData> ToImuData( const sensor_msgs::Imu::ConstPtr& msg); - void HandleImuMessage(const string& sensor_id, + void HandleImuMessage(const std::string& sensor_id, const sensor_msgs::Imu::ConstPtr& msg); - void HandleLaserScanMessage(const string& sensor_id, + void HandleLaserScanMessage(const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg); void HandleMultiEchoLaserScanMessage( - const string& sensor_id, + const std::string& sensor_id, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg); - void HandlePointCloud2Message(const string& sensor_id, + void HandlePointCloud2Message(const std::string& sensor_id, const sensor_msgs::PointCloud2::ConstPtr& msg); const TfBridge& tf_bridge() const; private: void HandleLaserScan( - const string& sensor_id, ::cartographer::common::Time start_time, - const string& frame_id, + const std::string& sensor_id, ::cartographer::common::Time start_time, + const std::string& frame_id, const ::cartographer::sensor::PointCloudWithIntensities& points); - void HandleRangefinder(const string& sensor_id, + void HandleRangefinder(const std::string& sensor_id, ::cartographer::common::Time time, - const string& frame_id, + const std::string& frame_id, const ::cartographer::sensor::TimedPointCloud& ranges); const int num_subdivisions_per_laser_scan_; diff --git a/cartographer_ros/cartographer_ros/start_trajectory_main.cc b/cartographer_ros/cartographer_ros/start_trajectory_main.cc index 7f81969..18ea230 100644 --- a/cartographer_ros/cartographer_ros/start_trajectory_main.cc +++ b/cartographer_ros/cartographer_ros/start_trajectory_main.cc @@ -43,8 +43,8 @@ namespace { TrajectoryOptions LoadOptions() { auto file_resolver = cartographer::common::make_unique< cartographer::common::ConfigurationFileResolver>( - std::vector{FLAGS_configuration_directory}); - const string code = + std::vector{FLAGS_configuration_directory}); + const std::string code = file_resolver->GetFileContentOrDie(FLAGS_configuration_basename); auto lua_parameter_dictionary = cartographer::common::LuaParameterDictionary::NonReferenceCounted( diff --git a/cartographer_ros/cartographer_ros/tf_bridge.cc b/cartographer_ros/cartographer_ros/tf_bridge.cc index d191f66..0985026 100644 --- a/cartographer_ros/cartographer_ros/tf_bridge.cc +++ b/cartographer_ros/cartographer_ros/tf_bridge.cc @@ -21,7 +21,7 @@ namespace cartographer_ros { -TfBridge::TfBridge(const string& tracking_frame, +TfBridge::TfBridge(const std::string& tracking_frame, const double lookup_transform_timeout_sec, const tf2_ros::Buffer* buffer) : tracking_frame_(tracking_frame), @@ -29,7 +29,8 @@ TfBridge::TfBridge(const string& tracking_frame, buffer_(buffer) {} std::unique_ptr<::cartographer::transform::Rigid3d> TfBridge::LookupToTracking( - const ::cartographer::common::Time time, const string& frame_id) const { + const ::cartographer::common::Time time, + const std::string& frame_id) const { ::ros::Duration timeout(lookup_transform_timeout_sec_); std::unique_ptr<::cartographer::transform::Rigid3d> frame_id_to_tracking; try { diff --git a/cartographer_ros/cartographer_ros/tf_bridge.h b/cartographer_ros/cartographer_ros/tf_bridge.h index 55edac7..878ce41 100644 --- a/cartographer_ros/cartographer_ros/tf_bridge.h +++ b/cartographer_ros/cartographer_ros/tf_bridge.h @@ -28,8 +28,8 @@ namespace cartographer_ros { class TfBridge { public: - TfBridge(const string& tracking_frame, double lookup_transform_timeout_sec, - const tf2_ros::Buffer* buffer); + TfBridge(const std::string& tracking_frame, + double lookup_transform_timeout_sec, const tf2_ros::Buffer* buffer); ~TfBridge() {} TfBridge(const TfBridge&) = delete; @@ -38,10 +38,10 @@ class TfBridge { // Returns the transform for 'frame_id' to 'tracking_frame_' if it exists at // 'time'. std::unique_ptr<::cartographer::transform::Rigid3d> LookupToTracking( - ::cartographer::common::Time time, const string& frame_id) const; + ::cartographer::common::Time time, const std::string& frame_id) const; private: - const string tracking_frame_; + const std::string tracking_frame_; const double lookup_transform_timeout_sec_; const tf2_ros::Buffer* const buffer_; }; diff --git a/cartographer_ros/cartographer_ros/time_conversion.cc b/cartographer_ros/cartographer_ros/time_conversion.cc index 6371c57..1075c40 100644 --- a/cartographer_ros/cartographer_ros/time_conversion.cc +++ b/cartographer_ros/cartographer_ros/time_conversion.cc @@ -22,8 +22,8 @@ namespace cartographer_ros { ::ros::Time ToRos(::cartographer::common::Time time) { - int64 uts_timestamp = ::cartographer::common::ToUniversal(time); - int64 ns_since_unix_epoch = + int64_t uts_timestamp = ::cartographer::common::ToUniversal(time); + int64_t ns_since_unix_epoch = (uts_timestamp - ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds * 10000000ll) * diff --git a/cartographer_ros/cartographer_ros/time_conversion_test.cc b/cartographer_ros/cartographer_ros/time_conversion_test.cc index 5f1b7bf..5ab36c5 100644 --- a/cartographer_ros/cartographer_ros/time_conversion_test.cc +++ b/cartographer_ros/cartographer_ros/time_conversion_test.cc @@ -26,9 +26,9 @@ namespace cartographer_ros { namespace { TEST(TimeConversion, testToRos) { - std::vector values = {0, 1469091375, 1466481821, 1462101382, + std::vector values = {0, 1469091375, 1466481821, 1462101382, 1468238899}; - for (int64 seconds_since_epoch : values) { + for (int64_t seconds_since_epoch : values) { ::ros::Time ros_now; ros_now.fromSec(seconds_since_epoch); ::cartographer::common::Time cartographer_now( diff --git a/cartographer_ros/cartographer_ros/trajectory_options.h b/cartographer_ros/cartographer_ros/trajectory_options.h index e3063ec..82a2926 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.h +++ b/cartographer_ros/cartographer_ros/trajectory_options.h @@ -29,9 +29,9 @@ namespace cartographer_ros { struct TrajectoryOptions { ::cartographer::mapping::proto::TrajectoryBuilderOptions trajectory_builder_options; - string tracking_frame; - string published_frame; - string odom_frame; + std::string tracking_frame; + std::string published_frame; + std::string odom_frame; bool provide_odom_frame; bool use_odometry; int num_laser_scans; diff --git a/cartographer_ros/cartographer_ros/urdf_reader.cc b/cartographer_ros/cartographer_ros/urdf_reader.cc index 7acf30d..e389e18 100644 --- a/cartographer_ros/cartographer_ros/urdf_reader.cc +++ b/cartographer_ros/cartographer_ros/urdf_reader.cc @@ -25,7 +25,7 @@ namespace cartographer_ros { std::vector ReadStaticTransformsFromUrdf( - const string& urdf_filename, tf2_ros::Buffer* const tf_buffer) { + const std::string& urdf_filename, tf2_ros::Buffer* const tf_buffer) { urdf::Model model; CHECK(model.initFile(urdf_filename)); #if URDFDOM_HEADERS_HAS_SHARED_PTR_DEFS diff --git a/cartographer_ros/cartographer_ros/urdf_reader.h b/cartographer_ros/cartographer_ros/urdf_reader.h index 2278c3a..bb34a0e 100644 --- a/cartographer_ros/cartographer_ros/urdf_reader.h +++ b/cartographer_ros/cartographer_ros/urdf_reader.h @@ -25,7 +25,7 @@ namespace cartographer_ros { std::vector ReadStaticTransformsFromUrdf( - const string& urdf_filename, tf2_ros::Buffer* tf_buffer); + const std::string& urdf_filename, tf2_ros::Buffer* tf_buffer); } // namespace cartographer_ros