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

25
cartographer_ros/cartographer_ros/assets_writer_main.cc
View File

@ -77,7 +77,7 @@ namespace carto = ::cartographer;
template <typename T> template <typename T>
std::unique_ptr<carto::io::PointsBatch> HandleMessage( 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 tf2_ros::Buffer& tf_buffer,
const carto::transform::TransformInterpolationBuffer& const carto::transform::TransformInterpolationBuffer&
transform_interpolation_buffer) { transform_interpolation_buffer) {
@ -116,15 +116,16 @@ std::unique_ptr<carto::io::PointsBatch> HandleMessage(
return points_batch; return points_batch;
} }
void Run(const string& pose_graph_filename, void Run(const std::string& pose_graph_filename,
const std::vector<string>& bag_filenames, const std::vector<std::string>& bag_filenames,
const string& configuration_directory, const std::string& configuration_directory,
const string& configuration_basename, const string& urdf_filename, const std::string& configuration_basename,
const string& output_file_prefix) { const std::string& urdf_filename,
const std::string& output_file_prefix) {
auto file_resolver = auto file_resolver =
carto::common::make_unique<carto::common::ConfigurationFileResolver>( carto::common::make_unique<carto::common::ConfigurationFileResolver>(
std::vector<string>{configuration_directory}); std::vector<std::string>{configuration_directory});
const string code = const std::string code =
file_resolver->GetFileContentOrDie(configuration_basename); file_resolver->GetFileContentOrDie(configuration_basename);
carto::common::LuaParameterDictionary lua_parameter_dictionary( carto::common::LuaParameterDictionary lua_parameter_dictionary(
code, std::move(file_resolver)); 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 " "trajectory in the same order as the correponding trajectories in the "
"pose graph proto."; "pose graph proto.";
const string file_prefix = !output_file_prefix.empty() const std::string file_prefix = !output_file_prefix.empty()
? output_file_prefix ? output_file_prefix
: bag_filenames.front() + "_"; : bag_filenames.front() + "_";
const auto file_writer_factory = [file_prefix](const string& filename) { const auto file_writer_factory = [file_prefix](const std::string& filename) {
return carto::common::make_unique<carto::io::StreamFileWriter>(file_prefix + return carto::common::make_unique<carto::io::StreamFileWriter>(file_prefix +
filename); filename);
}; };
@ -169,14 +170,14 @@ void Run(const string& pose_graph_filename,
builder.CreatePipeline( builder.CreatePipeline(
lua_parameter_dictionary.GetDictionary("pipeline").get()); lua_parameter_dictionary.GetDictionary("pipeline").get());
const string tracking_frame = const std::string tracking_frame =
lua_parameter_dictionary.GetString("tracking_frame"); lua_parameter_dictionary.GetString("tracking_frame");
do { do {
for (size_t trajectory_id = 0; trajectory_id < bag_filenames.size(); for (size_t trajectory_id = 0; trajectory_id < bag_filenames.size();
++trajectory_id) { ++trajectory_id) {
const carto::mapping::proto::Trajectory& trajectory_proto = const carto::mapping::proto::Trajectory& trajectory_proto =
pose_graph_proto.trajectory(trajectory_id); 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 << "..."; LOG(INFO) << "Processing " << bag_filename << "...";
if (trajectory_proto.node_size() == 0) { if (trajectory_proto.node_size() == 0) {
continue; continue;

2
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( 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 TrajectoryOptions& trajectory_options) {
const int trajectory_id = map_builder_.AddTrajectoryBuilder( const int trajectory_id = map_builder_.AddTrajectoryBuilder(
expected_sensor_ids, trajectory_options.trajectory_builder_options); expected_sensor_ids, trajectory_options.trajectory_builder_options);

4
cartographer_ros/cartographer_ros/map_builder_bridge.h
View File

@ -51,11 +51,11 @@ class MapBuilderBridge {
MapBuilderBridge& operator=(const MapBuilderBridge&) = delete; MapBuilderBridge& operator=(const MapBuilderBridge&) = delete;
void LoadMap(const std::string& map_filename); 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); const TrajectoryOptions& trajectory_options);
void FinishTrajectory(int trajectory_id); void FinishTrajectory(int trajectory_id);
void RunFinalOptimization(); void RunFinalOptimization();
void SerializeState(const string& filename); void SerializeState(const std::string& filename);
bool HandleSubmapQuery( bool HandleSubmapQuery(
cartographer_ros_msgs::SubmapQuery::Request& request, 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::sensor::PointCloudWithIntensities;
using ::cartographer::transform::Rigid3d; using ::cartographer::transform::Rigid3d;
sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64 timestamp, sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64_t timestamp,
const string& frame_id, const std::string& frame_id,
const int num_points) { const int num_points) {
sensor_msgs::PointCloud2 msg; sensor_msgs::PointCloud2 msg;
msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp)); msg.header.stamp = ToRos(::cartographer::common::FromUniversal(timestamp));
@ -140,7 +140,7 @@ bool PointCloud2HasField(const sensor_msgs::PointCloud2& pc2,
} // namespace } // namespace
sensor_msgs::PointCloud2 ToPointCloud2Message( 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) { const ::cartographer::sensor::TimedPointCloud& point_cloud) {
auto msg = PreparePointCloud2Message(timestamp, frame_id, point_cloud.size()); auto msg = PreparePointCloud2Message(timestamp, frame_id, point_cloud.size());
::ros::serialization::OStream stream(msg.data.data(), msg.data.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 { namespace cartographer_ros {
sensor_msgs::PointCloud2 ToPointCloud2Message( sensor_msgs::PointCloud2 ToPointCloud2Message(
int64 timestamp, const string& frame_id, int64_t timestamp, const std::string& frame_id,
const ::cartographer::sensor::TimedPointCloud& point_cloud); const ::cartographer::sensor::TimedPointCloud& point_cloud);
geometry_msgs::Transform ToGeometryMsgTransform( 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. // calls 'handler' on the 'node' to handle messages. Returns the subscriber.
template <typename MessageType> template <typename MessageType>
::ros::Subscriber SubscribeWithHandler( ::ros::Subscriber SubscribeWithHandler(
void (Node::*handler)(int, const string&, void (Node::*handler)(int, const std::string&,
const typename MessageType::ConstPtr&), 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) { ::ros::NodeHandle* const node_handle, Node* const node) {
return node_handle->subscribe<MessageType>( return node_handle->subscribe<MessageType>(
topic, kInfiniteSubscriberQueueSize, 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 TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics) { 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. // 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)) { topics.laser_scan_topic, options.num_laser_scans)) {
expected_topics.insert(topic); expected_topics.insert(topic);
} }
for (const string& topic : for (const std::string& topic :
ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic, ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic,
options.num_multi_echo_laser_scans)) { options.num_multi_echo_laser_scans)) {
expected_topics.insert(topic); expected_topics.insert(topic);
} }
for (const string& topic : ComputeRepeatedTopicNames( for (const std::string& topic : ComputeRepeatedTopicNames(
topics.point_cloud2_topic, options.num_point_clouds)) { topics.point_cloud2_topic, options.num_point_clouds)) {
expected_topics.insert(topic); expected_topics.insert(topic);
} }
@ -285,7 +285,7 @@ std::unordered_set<string> Node::ComputeExpectedTopics(
int Node::AddTrajectory(const TrajectoryOptions& options, int Node::AddTrajectory(const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics) { 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); ComputeExpectedTopics(options, topics);
const int trajectory_id = const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
@ -301,7 +301,7 @@ int Node::AddTrajectory(const TrajectoryOptions& options,
void Node::LaunchSubscribers(const TrajectoryOptions& options, void Node::LaunchSubscribers(const TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics, const cartographer_ros_msgs::SensorTopics& topics,
const int trajectory_id) { const int trajectory_id) {
for (const string& topic : ComputeRepeatedTopicNames( for (const std::string& topic : ComputeRepeatedTopicNames(
topics.laser_scan_topic, options.num_laser_scans)) { topics.laser_scan_topic, options.num_laser_scans)) {
subscribers_[trajectory_id].push_back( subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::LaserScan>( {SubscribeWithHandler<sensor_msgs::LaserScan>(
@ -309,7 +309,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
this), this),
topic}); topic});
} }
for (const string& topic : for (const std::string& topic :
ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic, ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic,
options.num_multi_echo_laser_scans)) { options.num_multi_echo_laser_scans)) {
subscribers_[trajectory_id].push_back( subscribers_[trajectory_id].push_back(
@ -318,7 +318,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
&node_handle_, this), &node_handle_, this),
topic}); topic});
} }
for (const string& topic : ComputeRepeatedTopicNames( for (const std::string& topic : ComputeRepeatedTopicNames(
topics.point_cloud2_topic, options.num_point_clouds)) { topics.point_cloud2_topic, options.num_point_clouds)) {
subscribers_[trajectory_id].push_back( subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::PointCloud2>( {SubscribeWithHandler<sensor_msgs::PointCloud2>(
@ -333,7 +333,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
(node_options_.map_builder_options.use_trajectory_builder_2d() && (node_options_.map_builder_options.use_trajectory_builder_2d() &&
options.trajectory_builder_options.trajectory_builder_2d_options() options.trajectory_builder_options.trajectory_builder_2d_options()
.use_imu_data())) { .use_imu_data())) {
string topic = topics.imu_topic; std::string topic = topics.imu_topic;
subscribers_[trajectory_id].push_back( subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage, {SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
trajectory_id, topic, trajectory_id, topic,
@ -342,7 +342,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
} }
if (options.use_odometry) { if (options.use_odometry) {
string topic = topics.odometry_topic; std::string topic = topics.odometry_topic;
subscribers_[trajectory_id].push_back( subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage, {SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
trajectory_id, topic, trajectory_id, topic,
@ -399,14 +399,14 @@ void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
AddTrajectory(options, DefaultSensorTopics()); AddTrajectory(options, DefaultSensorTopics());
} }
std::unordered_set<string> Node::ComputeDefaultTopics( std::unordered_set<std::string> Node::ComputeDefaultTopics(
const TrajectoryOptions& options) { const TrajectoryOptions& options) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
return ComputeExpectedTopics(options, DefaultSensorTopics()); return ComputeExpectedTopics(options, DefaultSensorTopics());
} }
int Node::AddOfflineTrajectory( int Node::AddOfflineTrajectory(
const std::unordered_set<string>& expected_sensor_ids, const std::unordered_set<std::string>& expected_sensor_ids,
const TrajectoryOptions& options) { const TrajectoryOptions& options) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
const int trajectory_id = const int trajectory_id =
@ -483,7 +483,7 @@ void Node::RunFinalOptimization() {
} }
void Node::HandleOdometryMessage(const int trajectory_id, void Node::HandleOdometryMessage(const int trajectory_id,
const string& sensor_id, const std::string& sensor_id,
const nav_msgs::Odometry::ConstPtr& msg) { const nav_msgs::Odometry::ConstPtr& msg) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) { 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); 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) { const sensor_msgs::Imu::ConstPtr& msg) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) { 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, void Node::HandleLaserScanMessage(const int trajectory_id,
const string& sensor_id, const std::string& sensor_id,
const sensor_msgs::LaserScan::ConstPtr& msg) { const sensor_msgs::LaserScan::ConstPtr& msg) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
@ -523,7 +524,7 @@ void Node::HandleLaserScanMessage(const int trajectory_id,
} }
void Node::HandleMultiEchoLaserScanMessage( void Node::HandleMultiEchoLaserScanMessage(
int trajectory_id, const string& sensor_id, int trajectory_id, const std::string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
@ -534,7 +535,7 @@ void Node::HandleMultiEchoLaserScanMessage(
} }
void Node::HandlePointCloud2Message( 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) { const sensor_msgs::PointCloud2::ConstPtr& msg) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
@ -544,7 +545,7 @@ void Node::HandlePointCloud2Message(
->HandlePointCloud2Message(sensor_id, msg); ->HandlePointCloud2Message(sensor_id, msg);
} }
void Node::SerializeState(const string& filename) { void Node::SerializeState(const std::string& filename) {
carto::common::MutexLocker lock(&mutex_); carto::common::MutexLocker lock(&mutex_);
map_builder_bridge_.SerializeState(filename); 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); void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);
// Compute the default topics for the given 'options'. // Compute the default topics for the given 'options'.
std::unordered_set<string> ComputeDefaultTopics( std::unordered_set<std::string> ComputeDefaultTopics(
const TrajectoryOptions& options); const TrajectoryOptions& options);
// Adds a trajectory for offline processing, i.e. not listening to topics. // Adds a trajectory for offline processing, i.e. not listening to topics.
int AddOfflineTrajectory( int AddOfflineTrajectory(
const std::unordered_set<string>& expected_sensor_ids, const std::unordered_set<std::string>& expected_sensor_ids,
const TrajectoryOptions& options); const TrajectoryOptions& options);
// The following functions handle adding sensor data to a trajectory. // 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); 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); 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); const sensor_msgs::LaserScan::ConstPtr& msg);
void HandleMultiEchoLaserScanMessage( void HandleMultiEchoLaserScanMessage(
int trajectory_id, const string& sensor_id, int trajectory_id, const std::string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg); 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); const sensor_msgs::PointCloud2::ConstPtr& msg);
// Serializes the complete Node state. // 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. // Loads a persisted state to use as a map.
void LoadMap(const std::string& map_filename); void LoadMap(const std::string& map_filename);
@ -97,10 +97,11 @@ class Node {
struct Subscriber { struct Subscriber {
::ros::Subscriber 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 // it was given in its constructor. Since we rely on the topic name as the
// unique identifier of a subscriber, we remember it ourselves. // unique identifier of a subscriber, we remember it ourselves.
string topic; std::string topic;
}; };
bool HandleSubmapQuery( bool HandleSubmapQuery(
@ -115,7 +116,7 @@ class Node {
bool HandleWriteState(cartographer_ros_msgs::WriteState::Request& request, bool HandleWriteState(cartographer_ros_msgs::WriteState::Request& request,
cartographer_ros_msgs::WriteState::Response& response); cartographer_ros_msgs::WriteState::Response& response);
// Returns the set of topic names we want to subscribe to. // 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 TrajectoryOptions& options,
const cartographer_ros_msgs::SensorTopics& topics); const cartographer_ros_msgs::SensorTopics& topics);
int AddTrajectory(const TrajectoryOptions& options, 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( std::tuple<NodeOptions, TrajectoryOptions> LoadOptions(
const string& configuration_directory, const std::string& configuration_directory,
const string& configuration_basename) { const std::string& configuration_basename) {
auto file_resolver = cartographer::common::make_unique< auto file_resolver = cartographer::common::make_unique<
cartographer::common::ConfigurationFileResolver>( cartographer::common::ConfigurationFileResolver>(
std::vector<string>{configuration_directory}); std::vector<std::string>{configuration_directory});
const string code = const std::string code =
file_resolver->GetFileContentOrDie(configuration_basename); file_resolver->GetFileContentOrDie(configuration_basename);
cartographer::common::LuaParameterDictionary lua_parameter_dictionary( cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
code, std::move(file_resolver)); 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. // Top-level options of Cartographer's ROS integration.
struct NodeOptions { struct NodeOptions {
::cartographer::mapping::proto::MapBuilderOptions map_builder_options; ::cartographer::mapping::proto::MapBuilderOptions map_builder_options;
string map_frame; std::string map_frame;
double lookup_transform_timeout_sec; double lookup_transform_timeout_sec;
double submap_publish_period_sec; double submap_publish_period_sec;
double pose_publish_period_sec; double pose_publish_period_sec;
@ -41,8 +41,8 @@ NodeOptions CreateNodeOptions(
::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary); ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary);
std::tuple<NodeOptions, TrajectoryOptions> LoadOptions( std::tuple<NodeOptions, TrajectoryOptions> LoadOptions(
const string& configuration_directory, const std::string& configuration_directory,
const string& configuration_basename); const std::string& configuration_basename);
} // namespace cartographer_ros } // namespace cartographer_ros

11
cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc
View File

@ -107,7 +107,7 @@ class Node {
private: private:
void HandleSubmapList(const cartographer_ros_msgs::SubmapList::ConstPtr& msg); void HandleSubmapList(const cartographer_ros_msgs::SubmapList::ConstPtr& msg);
void DrawAndPublish(const ::ros::WallTimerEvent& timer_event); 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::Array2f& origin,
const Eigen::Array2i& size, const Eigen::Array2i& size,
cairo_surface_t* surface); 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::Array2f& origin,
const Eigen::Array2i& size, const Eigen::Array2i& size,
cairo_surface_t* surface) { 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.y = 0.;
occupancy_grid.info.origin.orientation.z = 0.; occupancy_grid.info.origin.orientation.z = 0.;
const uint32* pixel_data = const uint32_t* pixel_data =
reinterpret_cast<uint32*>(cairo_image_surface_get_data(surface)); reinterpret_cast<uint32_t*>(cairo_image_surface_get_data(surface));
occupancy_grid.data.reserve(size.x() * size.y()); occupancy_grid.data.reserve(size.x() * size.y());
for (int y = size.y() - 1; y >= 0; --y) { for (int y = size.y() - 1; y >= 0; --y) {
for (int x = 0; x < size.x(); ++x) { 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 color = packed >> 16;
const unsigned char observed = packed >> 8; const unsigned char observed = packed >> 8;
const int value = const int value =

20
cartographer_ros/cartographer_ros/offline_node_main.cc
View File

@ -16,8 +16,8 @@
#include <errno.h> #include <errno.h>
#include <string.h> #include <string.h>
#include <sys/time.h>
#include <sys/resource.h> #include <sys/resource.h>
#include <sys/time.h>
#include <time.h> #include <time.h>
#include <chrono> #include <chrono>
#include <sstream> #include <sstream>
@ -68,7 +68,7 @@ constexpr char kTfTopic[] = "tf";
constexpr double kClockPublishFrequencySec = 1. / 30.; constexpr double kClockPublishFrequencySec = 1. / 30.;
constexpr int kSingleThreaded = 1; 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 = const std::chrono::time_point<std::chrono::steady_clock> start_time =
std::chrono::steady_clock::now(); std::chrono::steady_clock::now();
NodeOptions node_options; NodeOptions node_options;
@ -97,8 +97,9 @@ void Run(const std::vector<string>& bag_filenames) {
node.LoadMap(FLAGS_pbstream_filename); node.LoadMap(FLAGS_pbstream_filename);
} }
std::unordered_set<string> expected_sensor_ids; std::unordered_set<std::string> expected_sensor_ids;
for (const string& topic : node.ComputeDefaultTopics(trajectory_options)) { for (const std::string& topic :
node.ComputeDefaultTopics(trajectory_options)) {
CHECK(expected_sensor_ids.insert(node.node_handle()->resolveName(topic)) CHECK(expected_sensor_ids.insert(node.node_handle()->resolveName(topic))
.second); .second);
} }
@ -127,7 +128,7 @@ void Run(const std::vector<string>& bag_filenames) {
}, },
false /* oneshot */, false /* autostart */); false /* oneshot */, false /* autostart */);
for (const string& bag_filename : bag_filenames) { for (const std::string& bag_filename : bag_filenames) {
if (!::ros::ok()) { if (!::ros::ok()) {
break; break;
} }
@ -169,10 +170,9 @@ void Run(const std::vector<string>& bag_filenames) {
} }
while (!delayed_messages.empty() && while (!delayed_messages.empty() &&
delayed_messages.front().getTime() < delayed_messages.front().getTime() < msg.getTime() - kDelay) {
msg.getTime() - kDelay) {
const rosbag::MessageInstance& delayed_msg = delayed_messages.front(); 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 */); delayed_msg.getTopic(), false /* resolve */);
if (delayed_msg.isType<sensor_msgs::LaserScan>()) { if (delayed_msg.isType<sensor_msgs::LaserScan>()) {
node.HandleLaserScanMessage( node.HandleLaserScanMessage(
@ -208,7 +208,7 @@ void Run(const std::vector<string>& bag_filenames) {
delayed_messages.pop_front(); delayed_messages.pop_front();
} }
const string topic = const std::string topic =
node.node_handle()->resolveName(msg.getTopic(), false /* resolve */); node.node_handle()->resolveName(msg.getTopic(), false /* resolve */);
if (expected_sensor_ids.count(topic) == 0) { if (expected_sensor_ids.count(topic) == 0) {
continue; continue;
@ -245,7 +245,7 @@ void Run(const std::vector<string>& bag_filenames) {
#endif #endif
if (::ros::ok()) { 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 << "'..."; LOG(INFO) << "Writing state to '" << output_filename << "'...";
node.SerializeState(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, void WriteYaml(const ::cartographer::io::Image& image,
const ::cartographer::mapping_2d::MapLimits& limits, 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) { ::cartographer::io::FileWriter* file_writer) {
const double resolution = limits.resolution(); const double resolution = limits.resolution();
const double x_offset = const double x_offset =
@ -66,7 +66,8 @@ RosMapWritingPointsProcessor::RosMapWritingPointsProcessor(
const ::cartographer::mapping_2d::proto::RangeDataInserterOptions& const ::cartographer::mapping_2d::proto::RangeDataInserterOptions&
range_data_inserter_options, range_data_inserter_options,
::cartographer::io::FileWriterFactory file_writer_factory, ::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), : filestem_(filestem),
next_(next), next_(next),
file_writer_factory_(file_writer_factory), 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& const ::cartographer::mapping_2d::proto::RangeDataInserterOptions&
range_data_inserter_options, range_data_inserter_options,
::cartographer::io::FileWriterFactory file_writer_factory, ::cartographer::io::FileWriterFactory file_writer_factory,
const string& filestem, PointsProcessor* next); const std::string& filestem, PointsProcessor* next);
RosMapWritingPointsProcessor(const RosMapWritingPointsProcessor&) = delete; RosMapWritingPointsProcessor(const RosMapWritingPointsProcessor&) = delete;
RosMapWritingPointsProcessor& operator=(const RosMapWritingPointsProcessor&) = RosMapWritingPointsProcessor& operator=(const RosMapWritingPointsProcessor&) =
delete; delete;
@ -52,7 +52,7 @@ class RosMapWritingPointsProcessor
FlushResult Flush() override; FlushResult Flush() override;
private: private:
const string filestem_; const std::string filestem_;
PointsProcessor* const next_; PointsProcessor* const next_;
::cartographer::io::FileWriterFactory file_writer_factory_; ::cartographer::io::FileWriterFactory file_writer_factory_;
::cartographer::mapping_2d::RangeDataInserter range_data_inserter_; ::cartographer::mapping_2d::RangeDataInserter range_data_inserter_;

15
cartographer_ros/cartographer_ros/rosbag_validate_main.cc
View File

@ -47,17 +47,18 @@ namespace {
struct PerFrameId { struct PerFrameId {
ros::Time last_timestamp; ros::Time last_timestamp;
string topic; std::string topic;
::cartographer::common::Histogram histogram; ::cartographer::common::Histogram histogram;
std::unique_ptr<std::ofstream> timing_file; 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>( auto timing_file = ::cartographer::common::make_unique<std::ofstream>(
std::string("timing_") + frame_id + ".csv", std::ios_base::out); std::string("timing_") + frame_id + ".csv", std::ios_base::out);
(*timing_file) << "# Timing information for sensor with frame id: " (*timing_file)
<< frame_id << std::endl << "# Timing information for sensor with frame id: " << frame_id
<< std::endl
<< "# Columns are in order" << std::endl << "# Columns are in order" << std::endl
<< "# - packet index of the packet in the bag, first packet is 1" << "# - packet index of the packet in the bag, first packet is 1"
<< std::endl << std::endl
@ -75,16 +76,16 @@ std::unique_ptr<std::ofstream> CreateTimingFile(const string& frame_id) {
return timing_file; 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; rosbag::Bag bag;
bag.open(bag_filename, rosbag::bagmode::Read); bag.open(bag_filename, rosbag::bagmode::Read);
rosbag::View view(bag); rosbag::View view(bag);
std::map<string, PerFrameId> per_frame_id; std::map<std::string, PerFrameId> per_frame_id;
size_t message_index = 0; size_t message_index = 0;
for (const rosbag::MessageInstance& message : view) { for (const rosbag::MessageInstance& message : view) {
++message_index; ++message_index;
string frame_id; std::string frame_id;
ros::Time time; ros::Time time;
if (message.isType<sensor_msgs::PointCloud2>()) { if (message.isType<sensor_msgs::PointCloud2>()) {
auto msg = message.instantiate<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 { namespace {
const string& CheckNoLeadingSlash(const string& frame_id) { const std::string& CheckNoLeadingSlash(const std::string& frame_id) {
if (frame_id.size() > 0) { if (frame_id.size() > 0) {
CHECK_NE(frame_id[0], '/') << "The frame_id " << frame_id CHECK_NE(frame_id[0], '/') << "The frame_id " << frame_id
<< " should not start with a /. See 1.7 in " << " should not start with a /. See 1.7 in "
@ -40,7 +40,8 @@ const string& CheckNoLeadingSlash(const string& frame_id) {
} // namespace } // namespace
SensorBridge::SensorBridge( 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, const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer,
carto::mapping::TrajectoryBuilder* const trajectory_builder) carto::mapping::TrajectoryBuilder* const trajectory_builder)
: num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan), : 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( 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 = std::unique_ptr<::cartographer::sensor::OdometryData> odometry_data =
ToOdometryData(msg); ToOdometryData(msg);
if (odometry_data != nullptr) { if (odometry_data != nullptr) {
@ -101,7 +102,7 @@ std::unique_ptr<::cartographer::sensor::ImuData> SensorBridge::ToImuData(
sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)}); 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) { const sensor_msgs::Imu::ConstPtr& msg) {
std::unique_ptr<::cartographer::sensor::ImuData> imu_data = ToImuData(msg); std::unique_ptr<::cartographer::sensor::ImuData> imu_data = ToImuData(msg);
if (imu_data != nullptr) { if (imu_data != nullptr) {
@ -112,20 +113,21 @@ void SensorBridge::HandleImuMessage(const string& sensor_id,
} }
void SensorBridge::HandleLaserScanMessage( 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, HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
ToPointCloudWithIntensities(*msg)); ToPointCloudWithIntensities(*msg));
} }
void SensorBridge::HandleMultiEchoLaserScanMessage( void SensorBridge::HandleMultiEchoLaserScanMessage(
const string& sensor_id, const std::string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id, HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
ToPointCloudWithIntensities(*msg)); ToPointCloudWithIntensities(*msg));
} }
void SensorBridge::HandlePointCloud2Message( 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::PointCloud<pcl::PointXYZ> pcl_point_cloud;
pcl::fromROSMsg(*msg, pcl_point_cloud); pcl::fromROSMsg(*msg, pcl_point_cloud);
carto::sensor::TimedPointCloud point_cloud; carto::sensor::TimedPointCloud point_cloud;
@ -139,8 +141,8 @@ void SensorBridge::HandlePointCloud2Message(
const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; } const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; }
void SensorBridge::HandleLaserScan( void SensorBridge::HandleLaserScan(
const string& sensor_id, const carto::common::Time start_time, const std::string& sensor_id, const carto::common::Time start_time,
const string& frame_id, const std::string& frame_id,
const carto::sensor::PointCloudWithIntensities& points) { const carto::sensor::PointCloudWithIntensities& points) {
// TODO(gaschler): Use per-point time instead of subdivisions. // TODO(gaschler): Use per-point time instead of subdivisions.
for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) { for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
@ -162,8 +164,8 @@ void SensorBridge::HandleLaserScan(
} }
void SensorBridge::HandleRangefinder( void SensorBridge::HandleRangefinder(
const string& sensor_id, const carto::common::Time time, const std::string& sensor_id, const carto::common::Time time,
const string& frame_id, const carto::sensor::TimedPointCloud& ranges) { const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
const auto sensor_to_tracking = const auto sensor_to_tracking =
tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id)); tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
if (sensor_to_tracking != nullptr) { if (sensor_to_tracking != nullptr) {

20
cartographer_ros/cartographer_ros/sensor_bridge.h
View File

@ -40,7 +40,7 @@ namespace cartographer_ros {
class SensorBridge { class SensorBridge {
public: public:
explicit SensorBridge( 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, double lookup_transform_timeout_sec, tf2_ros::Buffer* tf_buffer,
::cartographer::mapping::TrajectoryBuilder* trajectory_builder); ::cartographer::mapping::TrajectoryBuilder* trajectory_builder);
@ -49,30 +49,30 @@ class SensorBridge {
std::unique_ptr<::cartographer::sensor::OdometryData> ToOdometryData( std::unique_ptr<::cartographer::sensor::OdometryData> ToOdometryData(
const nav_msgs::Odometry::ConstPtr& msg); 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); const nav_msgs::Odometry::ConstPtr& msg);
std::unique_ptr<::cartographer::sensor::ImuData> ToImuData( std::unique_ptr<::cartographer::sensor::ImuData> ToImuData(
const sensor_msgs::Imu::ConstPtr& msg); 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); 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); const sensor_msgs::LaserScan::ConstPtr& msg);
void HandleMultiEchoLaserScanMessage( void HandleMultiEchoLaserScanMessage(
const string& sensor_id, const std::string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg); 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 sensor_msgs::PointCloud2::ConstPtr& msg);
const TfBridge& tf_bridge() const; const TfBridge& tf_bridge() const;
private: private:
void HandleLaserScan( void HandleLaserScan(
const string& sensor_id, ::cartographer::common::Time start_time, const std::string& sensor_id, ::cartographer::common::Time start_time,
const string& frame_id, const std::string& frame_id,
const ::cartographer::sensor::PointCloudWithIntensities& points); const ::cartographer::sensor::PointCloudWithIntensities& points);
void HandleRangefinder(const string& sensor_id, void HandleRangefinder(const std::string& sensor_id,
::cartographer::common::Time time, ::cartographer::common::Time time,
const string& frame_id, const std::string& frame_id,
const ::cartographer::sensor::TimedPointCloud& ranges); const ::cartographer::sensor::TimedPointCloud& ranges);
const int num_subdivisions_per_laser_scan_; 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() { TrajectoryOptions LoadOptions() {
auto file_resolver = cartographer::common::make_unique< auto file_resolver = cartographer::common::make_unique<
cartographer::common::ConfigurationFileResolver>( cartographer::common::ConfigurationFileResolver>(
std::vector<string>{FLAGS_configuration_directory}); std::vector<std::string>{FLAGS_configuration_directory});
const string code = const std::string code =
file_resolver->GetFileContentOrDie(FLAGS_configuration_basename); file_resolver->GetFileContentOrDie(FLAGS_configuration_basename);
auto lua_parameter_dictionary = auto lua_parameter_dictionary =
cartographer::common::LuaParameterDictionary::NonReferenceCounted( cartographer::common::LuaParameterDictionary::NonReferenceCounted(

5
cartographer_ros/cartographer_ros/tf_bridge.cc
View File

@ -21,7 +21,7 @@
namespace cartographer_ros { namespace cartographer_ros {
TfBridge::TfBridge(const string& tracking_frame, TfBridge::TfBridge(const std::string& tracking_frame,
const double lookup_transform_timeout_sec, const double lookup_transform_timeout_sec,
const tf2_ros::Buffer* buffer) const tf2_ros::Buffer* buffer)
: tracking_frame_(tracking_frame), : tracking_frame_(tracking_frame),
@ -29,7 +29,8 @@ TfBridge::TfBridge(const string& tracking_frame,
buffer_(buffer) {} buffer_(buffer) {}
std::unique_ptr<::cartographer::transform::Rigid3d> TfBridge::LookupToTracking( 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_); ::ros::Duration timeout(lookup_transform_timeout_sec_);
std::unique_ptr<::cartographer::transform::Rigid3d> frame_id_to_tracking; std::unique_ptr<::cartographer::transform::Rigid3d> frame_id_to_tracking;
try { try {

8
cartographer_ros/cartographer_ros/tf_bridge.h
View File

@ -28,8 +28,8 @@ namespace cartographer_ros {
class TfBridge { class TfBridge {
public: public:
TfBridge(const string& tracking_frame, double lookup_transform_timeout_sec, TfBridge(const std::string& tracking_frame,
const tf2_ros::Buffer* buffer); double lookup_transform_timeout_sec, const tf2_ros::Buffer* buffer);
~TfBridge() {} ~TfBridge() {}
TfBridge(const TfBridge&) = delete; TfBridge(const TfBridge&) = delete;
@ -38,10 +38,10 @@ class TfBridge {
// Returns the transform for 'frame_id' to 'tracking_frame_' if it exists at // Returns the transform for 'frame_id' to 'tracking_frame_' if it exists at
// 'time'. // 'time'.
std::unique_ptr<::cartographer::transform::Rigid3d> LookupToTracking( 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: private:
const string tracking_frame_; const std::string tracking_frame_;
const double lookup_transform_timeout_sec_; const double lookup_transform_timeout_sec_;
const tf2_ros::Buffer* const buffer_; const tf2_ros::Buffer* const buffer_;
}; };

4
cartographer_ros/cartographer_ros/time_conversion.cc
View File

@ -22,8 +22,8 @@
namespace cartographer_ros { namespace cartographer_ros {
::ros::Time ToRos(::cartographer::common::Time time) { ::ros::Time ToRos(::cartographer::common::Time time) {
int64 uts_timestamp = ::cartographer::common::ToUniversal(time); int64_t uts_timestamp = ::cartographer::common::ToUniversal(time);
int64 ns_since_unix_epoch = int64_t ns_since_unix_epoch =
(uts_timestamp - (uts_timestamp -
::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds * ::cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds *
10000000ll) * 10000000ll) *

4
cartographer_ros/cartographer_ros/time_conversion_test.cc
View File

@ -26,9 +26,9 @@ namespace cartographer_ros {
namespace { namespace {
TEST(TimeConversion, testToRos) { TEST(TimeConversion, testToRos) {
std::vector<int64> values = {0, 1469091375, 1466481821, 1462101382, std::vector<int64_t> values = {0, 1469091375, 1466481821, 1462101382,
1468238899}; 1468238899};
for (int64 seconds_since_epoch : values) { for (int64_t seconds_since_epoch : values) {
::ros::Time ros_now; ::ros::Time ros_now;
ros_now.fromSec(seconds_since_epoch); ros_now.fromSec(seconds_since_epoch);
::cartographer::common::Time cartographer_now( ::cartographer::common::Time cartographer_now(

6
cartographer_ros/cartographer_ros/trajectory_options.h
View File

@ -29,9 +29,9 @@ namespace cartographer_ros {
struct TrajectoryOptions { struct TrajectoryOptions {
::cartographer::mapping::proto::TrajectoryBuilderOptions ::cartographer::mapping::proto::TrajectoryBuilderOptions
trajectory_builder_options; trajectory_builder_options;
string tracking_frame; std::string tracking_frame;
string published_frame; std::string published_frame;
string odom_frame; std::string odom_frame;
bool provide_odom_frame; bool provide_odom_frame;
bool use_odometry; bool use_odometry;
int num_laser_scans; int num_laser_scans;

2
cartographer_ros/cartographer_ros/urdf_reader.cc
View File

@ -25,7 +25,7 @@
namespace cartographer_ros { namespace cartographer_ros {
std::vector<geometry_msgs::TransformStamped> ReadStaticTransformsFromUrdf( 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; urdf::Model model;
CHECK(model.initFile(urdf_filename)); CHECK(model.initFile(urdf_filename));
#if URDFDOM_HEADERS_HAS_SHARED_PTR_DEFS #if URDFDOM_HEADERS_HAS_SHARED_PTR_DEFS

2
cartographer_ros/cartographer_ros/urdf_reader.h
View File

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