From da4b3e76396e8427171f39688f0eb6289bd05b95 Mon Sep 17 00:00:00 2001 From: Michael Grupp Date: Tue, 2 Apr 2019 12:03:43 +0200 Subject: [PATCH] Replace a few string operator+ by absl::StrCat(). (#1244) ...in some places that can be called frequently. --- .../cartographer_ros/map_builder_bridge.cc | 3 ++- cartographer_ros/cartographer_ros/node.cc | 23 +++++++++++-------- cartographer_ros/cartographer_ros/ros_map.cc | 18 +++++++-------- .../cartographer_rviz/ogre_slice.cc | 16 ++++++------- 4 files changed, 32 insertions(+), 28 deletions(-) diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index dc6760f..ea51b9a 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/cartographer_ros/cartographer_ros/map_builder_bridge.cc @@ -17,6 +17,7 @@ #include "cartographer_ros/map_builder_bridge.h" #include "absl/memory/memory.h" +#include "absl/strings/str_cat.h" #include "cartographer/io/color.h" #include "cartographer/io/proto_stream.h" #include "cartographer/mapping/pose_graph.h" @@ -45,7 +46,7 @@ constexpr double kConstraintMarkerScale = 0.025; visualization_msgs::Marker CreateTrajectoryMarker(const int trajectory_id, const std::string& frame_id) { visualization_msgs::Marker marker; - marker.ns = "Trajectory " + std::to_string(trajectory_id); + marker.ns = absl::StrCat("Trajectory ", trajectory_id); marker.id = 0; marker.type = visualization_msgs::Marker::LINE_STRIP; marker.header.stamp = ::ros::Time::now(); diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 8c988b3..48a68c3 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -22,6 +22,7 @@ #include "Eigen/Core" #include "absl/memory/memory.h" +#include "absl/strings/str_cat.h" #include "cartographer/common/configuration_file_resolver.h" #include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/port.h" @@ -471,8 +472,8 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock( cartographer_ros_msgs::StatusResponse status_response; if (trajectories_scheduled_for_finish_.count(trajectory_id)) { - const std::string message = "Trajectory " + std::to_string(trajectory_id) + - " already pending to finish."; + const std::string message = absl::StrCat("Trajectory ", trajectory_id, + " already pending to finish."); status_response.code = cartographer_ros_msgs::StatusCode::OK; status_response.message = message; LOG(INFO) << message; @@ -482,21 +483,21 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock( // First, check if we can actually finish the trajectory. if (!(trajectory_states.count(trajectory_id))) { const std::string error = - "Trajectory " + std::to_string(trajectory_id) + " doesn't exist."; + absl::StrCat("Trajectory ", trajectory_id, " doesn't exist."); LOG(ERROR) << error; status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND; status_response.message = error; return status_response; } else if (trajectory_states.at(trajectory_id) == TrajectoryState::FROZEN) { const std::string error = - "Trajectory " + std::to_string(trajectory_id) + " is frozen."; + absl::StrCat("Trajectory ", trajectory_id, " is frozen."); LOG(ERROR) << error; status_response.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; status_response.message = error; return status_response; } else if (trajectory_states.at(trajectory_id) == TrajectoryState::FINISHED) { - const std::string error = "Trajectory " + std::to_string(trajectory_id) + - " has already been finished."; + const std::string error = absl::StrCat("Trajectory ", trajectory_id, + " has already been finished."); LOG(ERROR) << error; status_response.code = cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED; @@ -504,7 +505,7 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock( return status_response; } else if (trajectory_states.at(trajectory_id) == TrajectoryState::DELETED) { const std::string error = - "Trajectory " + std::to_string(trajectory_id) + " has been deleted."; + absl::StrCat("Trajectory ", trajectory_id, " has been deleted."); LOG(ERROR) << error; status_response.code = cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED; @@ -525,7 +526,7 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock( map_builder_bridge_.FinishTrajectory(trajectory_id); trajectories_scheduled_for_finish_.emplace(trajectory_id); const std::string message = - "Finished trajectory " + std::to_string(trajectory_id) + "."; + absl::StrCat("Finished trajectory ", trajectory_id, "."); status_response.code = cartographer_ros_msgs::StatusCode::OK; status_response.message = message; return status_response; @@ -641,10 +642,12 @@ bool Node::HandleWriteState( if (map_builder_bridge_.SerializeState(request.filename, request.include_unfinished_submaps)) { response.status.code = cartographer_ros_msgs::StatusCode::OK; - response.status.message = "State written to '" + request.filename + "'."; + response.status.message = + absl::StrCat("State written to '", request.filename, "'."); } else { response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; - response.status.message = "Failed to write '" + request.filename + "'."; + response.status.message = + absl::StrCat("Failed to write '", request.filename, "'."); } return true; } diff --git a/cartographer_ros/cartographer_ros/ros_map.cc b/cartographer_ros/cartographer_ros/ros_map.cc index 82f9776..c5c0a1f 100644 --- a/cartographer_ros/cartographer_ros/ros_map.cc +++ b/cartographer_ros/cartographer_ros/ros_map.cc @@ -16,14 +16,15 @@ #include "cartographer_ros/ros_map.h" +#include "absl/strings/str_cat.h" + namespace cartographer_ros { void WritePgm(const ::cartographer::io::Image& image, const double resolution, ::cartographer::io::FileWriter* file_writer) { - const std::string header = "P5\n# Cartographer map; " + - std::to_string(resolution) + " m/pixel\n" + - std::to_string(image.width()) + " " + - std::to_string(image.height()) + "\n255\n"; + const std::string header = + absl::StrCat("P5\n# Cartographer map; ", resolution, " m/pixel\n", + image.width(), " ", image.height(), "\n255\n"); file_writer->Write(header.data(), header.size()); for (int y = 0; y < image.height(); ++y) { for (int x = 0; x < image.width(); ++x) { @@ -38,11 +39,10 @@ void WriteYaml(const double resolution, const Eigen::Vector2d& origin, ::cartographer::io::FileWriter* file_writer) { // Magic constants taken directly from ros map_saver code: // https://github.com/ros-planning/navigation/blob/ac41d2480c4cf1602daf39a6e9629142731d92b0/map_server/src/map_saver.cpp#L114 - const std::string output = - "image: " + pgm_filename + "\n" + - "resolution: " + std::to_string(resolution) + "\n" + "origin: [" + - std::to_string(origin.x()) + ", " + std::to_string(origin.y()) + - ", 0.0]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n"; + const std::string output = absl::StrCat( + "image: ", pgm_filename, "\n", "resolution: ", resolution, "\n", + "origin: [", origin.x(), ", ", origin.y(), + ", 0.0]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n"); file_writer->Write(output.data(), output.size()); } diff --git a/cartographer_rviz/cartographer_rviz/ogre_slice.cc b/cartographer_rviz/cartographer_rviz/ogre_slice.cc index b944018..3062042 100644 --- a/cartographer_rviz/cartographer_rviz/ogre_slice.cc +++ b/cartographer_rviz/cartographer_rviz/ogre_slice.cc @@ -24,6 +24,7 @@ #include "OgreMaterialManager.h" #include "OgreTechnique.h" #include "OgreTextureManager.h" +#include "absl/strings/str_cat.h" #include "cartographer/common/port.h" namespace cartographer_rviz { @@ -37,9 +38,8 @@ constexpr char kSubmapTexturePrefix[] = "SubmapTexture"; std::string GetSliceIdentifier( const ::cartographer::mapping::SubmapId& submap_id, const int slice_id) { - return std::to_string(submap_id.trajectory_id) + "-" + - std::to_string(submap_id.submap_index) + "-" + - std::to_string(slice_id); + return absl::StrCat(submap_id.trajectory_id, "-", submap_id.submap_index, "-", + slice_id); } } // namespace @@ -60,12 +60,12 @@ OgreSlice::OgreSlice(const ::cartographer::mapping::SubmapId& id, int slice_id, scene_manager_(scene_manager), submap_node_(submap_node), slice_node_(submap_node_->createChildSceneNode()), - manual_object_(scene_manager_->createManualObject( - kManualObjectPrefix + GetSliceIdentifier(id, slice_id))) { + manual_object_(scene_manager_->createManualObject(absl::StrCat( + kManualObjectPrefix, GetSliceIdentifier(id, slice_id)))) { material_ = Ogre::MaterialManager::getSingleton().getByName( kSubmapSourceMaterialName); - material_ = material_->clone(kSubmapMaterialPrefix + - GetSliceIdentifier(id_, slice_id_)); + material_ = material_->clone( + absl::StrCat(kSubmapMaterialPrefix, GetSliceIdentifier(id_, slice_id_))); material_->setReceiveShadows(false); material_->getTechnique(0)->setLightingEnabled(false); material_->setCullingMode(Ogre::CULL_NONE); @@ -126,7 +126,7 @@ void OgreSlice::Update( texture_.setNull(); } const std::string texture_name = - kSubmapTexturePrefix + GetSliceIdentifier(id_, slice_id_); + absl::StrCat(kSubmapTexturePrefix, GetSliceIdentifier(id_, slice_id_)); texture_ = Ogre::TextureManager::getSingleton().loadRawData( texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, pixel_stream, submap_texture.width, submap_texture.height,