12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Follow googlecartographer/cartographer#637. (#583) 

Use c++ standard types instead of the values from port.h.
master
Holger Rapp 2017-11-08 10:37:02 +01:00 committed by GitHub
parent e63126f329
commit ff92e9e4bd
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
24 changed files with 147 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
cartographer_ros/cartographer_ros/assets_writer_main.cc
View File

@ -77,7 +77,7 @@ namespace carto = ::cartographer;
template <typename T>
std::unique_ptr<carto::io::PointsBatch> 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<carto::io::PointsBatch> HandleMessage(
return points_batch;
}
void Run(const string& pose_graph_filename,
const std::vector<string>& 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<std::string>& 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<carto::common::ConfigurationFileResolver>(
std::vector<string>{configuration_directory});
const string code =
std::vector<std::string>{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<carto::io::StreamFileWriter>(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;

4
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -75,7 +75,7 @@ void MapBuilderBridge::LoadMap(const std::string& map_filename) {
}
int MapBuilderBridge::AddTrajectory(
const std::unordered_set<string>& expected_sensor_ids,
const std::unordered_set<std::string>& 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;

4
cartographer_ros/cartographer_ros/map_builder_bridge.h
View File

@ -51,11 +51,11 @@ class MapBuilderBridge {
MapBuilderBridge& operator=(const MapBuilderBridge&) = delete;
void LoadMap(const std::string& map_filename);
int AddTrajectory(const std::unordered_set<string>& expected_sensor_ids,
int AddTrajectory(const std::unordered_set<std::string>& 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,

6
cartographer_ros/cartographer_ros/msg_conversion.cc
View File

@ -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());

2
cartographer_ros/cartographer_ros/msg_conversion.h
View File

@ -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(

43
cartographer_ros/cartographer_ros/node.cc
View File

@ -60,9 +60,9 @@ cartographer_ros_msgs::SensorTopics DefaultSensorTopics() {
// calls 'handler' on the 'node' to handle messages. Returns the subscriber.
template <typename MessageType>
::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<MessageType>(
topic, kInfiniteSubscriberQueueSize,
@ -250,21 +250,21 @@ void Node::PublishConstraintList(
}
}
std::unordered_set<string> Node::ComputeExpectedTopics(
std::unordered_set<std::string> Node::ComputeExpectedTopics(
const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics) {
std::unordered_set<string> expected_topics;
std::unordered_set<std::string> 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<string> Node::ComputeExpectedTopics(
int Node::AddTrajectory(const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics) {
const std::unordered_set<string> expected_sensor_ids =
const std::unordered_set<std::string> 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<sensor_msgs::LaserScan>(
@ -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<sensor_msgs::PointCloud2>(
@ -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<sensor_msgs::Imu>(&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<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
trajectory_id, topic,
@ -399,14 +399,14 @@ void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
AddTrajectory(options, DefaultSensorTopics());
}
std::unordered_set<string> Node::ComputeDefaultTopics(
std::unordered_set<std::string> Node::ComputeDefaultTopics(
const TrajectoryOptions& options) {
carto::common::MutexLocker lock(&mutex_);
return ComputeExpectedTopics(options, DefaultSensorTopics());
}
int Node::AddOfflineTrajectory(
const std::unordered_set<string>& expected_sensor_ids,
const std::unordered_set<std::string>& 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);
}

23
cartographer_ros/cartographer_ros/node.h
View File

@ -64,29 +64,29 @@ class Node {
void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);
// Compute the default topics for the given 'options'.
std::unordered_set<string> ComputeDefaultTopics(
std::unordered_set<std::string> ComputeDefaultTopics(
const TrajectoryOptions& options);
// Adds a trajectory for offline processing, i.e. not listening to topics.
int AddOfflineTrajectory(
const std::unordered_set<string>& expected_sensor_ids,
const std::unordered_set<std::string>& 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<string> ComputeExpectedTopics(
std::unordered_set<std::string> ComputeExpectedTopics(
const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics);
int AddTrajectory(const TrajectoryOptions& options,

8
cartographer_ros/cartographer_ros/node_options.cc
View File

@ -44,12 +44,12 @@ NodeOptions CreateNodeOptions(
}
std::tuple<NodeOptions, TrajectoryOptions> 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<string>{configuration_directory});
const string code =
std::vector<std::string>{configuration_directory});
const std::string code =
file_resolver->GetFileContentOrDie(configuration_basename);
cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
code, std::move(file_resolver));

6
cartographer_ros/cartographer_ros/node_options.h
View File

@ -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<NodeOptions, TrajectoryOptions> LoadOptions(
const string& configuration_directory,
const string& configuration_basename);
const std::string& configuration_directory,
const std::string& configuration_basename);
} // namespace cartographer_ros

11
cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc
View File

@ -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<uint32*>(cairo_image_surface_get_data(surface));
const uint32_t* pixel_data =
reinterpret_cast<uint32_t*>(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 =

20
cartographer_ros/cartographer_ros/offline_node_main.cc
View File

@ -16,8 +16,8 @@
#include <errno.h>
#include <string.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <time.h>
#include <chrono>
#include <sstream>
@ -68,7 +68,7 @@ constexpr char kTfTopic[] = "tf";
constexpr double kClockPublishFrequencySec = 1. / 30.;
constexpr int kSingleThreaded = 1;
void Run(const std::vector<string>& bag_filenames) {
void Run(const std::vector<std::string>& bag_filenames) {
const std::chrono::time_point<std::chrono::steady_clock> start_time =
std::chrono::steady_clock::now();
NodeOptions node_options;
@ -97,8 +97,9 @@ void Run(const std::vector<string>& bag_filenames) {
node.LoadMap(FLAGS_pbstream_filename);
}
std::unordered_set<string> expected_sensor_ids;
for (const string& topic : node.ComputeDefaultTopics(trajectory_options)) {
std::unordered_set<std::string> 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<string>& 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<string>& 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<sensor_msgs::LaserScan>()) {
node.HandleLaserScanMessage(
@ -208,7 +208,7 @@ void Run(const std::vector<string>& 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<string>& 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);
}

5
cartographer_ros/cartographer_ros/ros_map_writing_points_processor.cc
View File

@ -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),

4
cartographer_ros/cartographer_ros/ros_map_writing_points_processor.h
View File

@ -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_;

41
cartographer_ros/cartographer_ros/rosbag_validate_main.cc
View File

@ -47,44 +47,45 @@ namespace {
struct PerFrameId {
ros::Time last_timestamp;
string topic;
std::string topic;
::cartographer::common::Histogram histogram;
std::unique_ptr<std::ofstream> timing_file;
};
std::unique_ptr<std::ofstream> CreateTimingFile(const string& frame_id) {
std::unique_ptr<std::ofstream> CreateTimingFile(const std::string& frame_id) {
auto timing_file = ::cartographer::common::make_unique<std::ofstream>(
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(<filename>, 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(<filename>, 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<string, PerFrameId> per_frame_id;
std::map<std::string, PerFrameId> 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<sensor_msgs::PointCloud2>()) {
auto msg = message.instantiate<sensor_msgs::PointCloud2>();

24
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

@ -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::PointXYZ> 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) {

20
cartographer_ros/cartographer_ros/sensor_bridge.h
View File

@ -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_;

4
cartographer_ros/cartographer_ros/start_trajectory_main.cc
View File

@ -43,8 +43,8 @@ namespace {
TrajectoryOptions LoadOptions() {
auto file_resolver = cartographer::common::make_unique<
cartographer::common::ConfigurationFileResolver>(
std::vector<string>{FLAGS_configuration_directory});
const string code =
std::vector<std::string>{FLAGS_configuration_directory});
const std::string code =
file_resolver->GetFileContentOrDie(FLAGS_configuration_basename);
auto lua_parameter_dictionary =
cartographer::common::LuaParameterDictionary::NonReferenceCounted(

5
cartographer_ros/cartographer_ros/tf_bridge.cc
View File

@ -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 {

8
cartographer_ros/cartographer_ros/tf_bridge.h
View File

@ -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_;
};

4
cartographer_ros/cartographer_ros/time_conversion.cc
View File

@ -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) *

4
cartographer_ros/cartographer_ros/time_conversion_test.cc
View File

@ -26,9 +26,9 @@ namespace cartographer_ros {
namespace {
TEST(TimeConversion, testToRos) {
std::vector<int64> values = {0, 1469091375, 1466481821, 1462101382,
std::vector<int64_t> 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(

6
cartographer_ros/cartographer_ros/trajectory_options.h
View File

@ -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;

2
cartographer_ros/cartographer_ros/urdf_reader.cc
View File

@ -25,7 +25,7 @@
namespace cartographer_ros {
std::vector<geometry_msgs::TransformStamped> 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

2
cartographer_ros/cartographer_ros/urdf_reader.h
View File

@ -25,7 +25,7 @@
namespace cartographer_ros {
std::vector<geometry_msgs::TransformStamped> ReadStaticTransformsFromUrdf(
const string& urdf_filename, tf2_ros::Buffer* tf_buffer);
const std::string& urdf_filename, tf2_ros::Buffer* tf_buffer);
} // namespace cartographer_ros