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

Replace a few string operator+ by absl::StrCat(). (#1244) 

...in some places that can be called frequently.
master
Michael Grupp 2019-04-02 12:03:43 +02:00 committed by Wally B. Feed
parent eb86ba6e7c
commit da4b3e7639
4 changed files with 32 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -17,6 +17,7 @@
#include "cartographer_ros/map_builder_bridge.h" #include "cartographer_ros/map_builder_bridge.h"
#include "absl/memory/memory.h" #include "absl/memory/memory.h"
#include "absl/strings/str_cat.h"
#include "cartographer/io/color.h" #include "cartographer/io/color.h"
#include "cartographer/io/proto_stream.h" #include "cartographer/io/proto_stream.h"
#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph.h"
@ -45,7 +46,7 @@ constexpr double kConstraintMarkerScale = 0.025;
visualization_msgs::Marker CreateTrajectoryMarker(const int trajectory_id, visualization_msgs::Marker CreateTrajectoryMarker(const int trajectory_id,
const std::string& frame_id) { const std::string& frame_id) {
visualization_msgs::Marker marker; visualization_msgs::Marker marker;
marker.ns = "Trajectory " + std::to_string(trajectory_id); marker.ns = absl::StrCat("Trajectory ", trajectory_id);
marker.id = 0; marker.id = 0;
marker.type = visualization_msgs::Marker::LINE_STRIP; marker.type = visualization_msgs::Marker::LINE_STRIP;
marker.header.stamp = ::ros::Time::now(); marker.header.stamp = ::ros::Time::now();

23
cartographer_ros/cartographer_ros/node.cc
View File

@ -22,6 +22,7 @@
#include "Eigen/Core" #include "Eigen/Core"
#include "absl/memory/memory.h" #include "absl/memory/memory.h"
#include "absl/strings/str_cat.h"
#include "cartographer/common/configuration_file_resolver.h" #include "cartographer/common/configuration_file_resolver.h"
#include "cartographer/common/lua_parameter_dictionary.h" #include "cartographer/common/lua_parameter_dictionary.h"
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
@ -471,8 +472,8 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
cartographer_ros_msgs::StatusResponse status_response; cartographer_ros_msgs::StatusResponse status_response;
if (trajectories_scheduled_for_finish_.count(trajectory_id)) { if (trajectories_scheduled_for_finish_.count(trajectory_id)) {
const std::string message = "Trajectory " + std::to_string(trajectory_id) + const std::string message = absl::StrCat("Trajectory ", trajectory_id,
" already pending to finish."; " already pending to finish.");
status_response.code = cartographer_ros_msgs::StatusCode::OK; status_response.code = cartographer_ros_msgs::StatusCode::OK;
status_response.message = message; status_response.message = message;
LOG(INFO) << message; LOG(INFO) << message;
@ -482,21 +483,21 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
// First, check if we can actually finish the trajectory. // First, check if we can actually finish the trajectory.
if (!(trajectory_states.count(trajectory_id))) { if (!(trajectory_states.count(trajectory_id))) {
const std::string error = const std::string error =
"Trajectory " + std::to_string(trajectory_id) + " doesn't exist."; absl::StrCat("Trajectory ", trajectory_id, " doesn't exist.");
LOG(ERROR) << error; LOG(ERROR) << error;
status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND; status_response.code = cartographer_ros_msgs::StatusCode::NOT_FOUND;
status_response.message = error; status_response.message = error;
return status_response; return status_response;
} else if (trajectory_states.at(trajectory_id) == TrajectoryState::FROZEN) { } else if (trajectory_states.at(trajectory_id) == TrajectoryState::FROZEN) {
const std::string error = const std::string error =
"Trajectory " + std::to_string(trajectory_id) + " is frozen."; absl::StrCat("Trajectory ", trajectory_id, " is frozen.");
LOG(ERROR) << error; LOG(ERROR) << error;
status_response.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; status_response.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
status_response.message = error; status_response.message = error;
return status_response; return status_response;
} else if (trajectory_states.at(trajectory_id) == TrajectoryState::FINISHED) { } else if (trajectory_states.at(trajectory_id) == TrajectoryState::FINISHED) {
const std::string error = "Trajectory " + std::to_string(trajectory_id) + const std::string error = absl::StrCat("Trajectory ", trajectory_id,
" has already been finished."; " has already been finished.");
LOG(ERROR) << error; LOG(ERROR) << error;
status_response.code = status_response.code =
cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED; cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED;
@ -504,7 +505,7 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
return status_response; return status_response;
} else if (trajectory_states.at(trajectory_id) == TrajectoryState::DELETED) { } else if (trajectory_states.at(trajectory_id) == TrajectoryState::DELETED) {
const std::string error = const std::string error =
"Trajectory " + std::to_string(trajectory_id) + " has been deleted."; absl::StrCat("Trajectory ", trajectory_id, " has been deleted.");
LOG(ERROR) << error; LOG(ERROR) << error;
status_response.code = status_response.code =
cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED; cartographer_ros_msgs::StatusCode::RESOURCE_EXHAUSTED;
@ -525,7 +526,7 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
map_builder_bridge_.FinishTrajectory(trajectory_id); map_builder_bridge_.FinishTrajectory(trajectory_id);
trajectories_scheduled_for_finish_.emplace(trajectory_id); trajectories_scheduled_for_finish_.emplace(trajectory_id);
const std::string message = 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.code = cartographer_ros_msgs::StatusCode::OK;
status_response.message = message; status_response.message = message;
return status_response; return status_response;
@ -641,10 +642,12 @@ bool Node::HandleWriteState(
if (map_builder_bridge_.SerializeState(request.filename, if (map_builder_bridge_.SerializeState(request.filename,
request.include_unfinished_submaps)) { request.include_unfinished_submaps)) {
response.status.code = cartographer_ros_msgs::StatusCode::OK; 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 { } else {
response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT; 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; return true;
} }

18
cartographer_ros/cartographer_ros/ros_map.cc
View File

@ -16,14 +16,15 @@
#include "cartographer_ros/ros_map.h" #include "cartographer_ros/ros_map.h"
#include "absl/strings/str_cat.h"
namespace cartographer_ros { namespace cartographer_ros {
void WritePgm(const ::cartographer::io::Image& image, const double resolution, void WritePgm(const ::cartographer::io::Image& image, const double resolution,
::cartographer::io::FileWriter* file_writer) { ::cartographer::io::FileWriter* file_writer) {
const std::string header = "P5\n# Cartographer map; " + const std::string header =
std::to_string(resolution) + " m/pixel\n" + absl::StrCat("P5\n# Cartographer map; ", resolution, " m/pixel\n",
std::to_string(image.width()) + " " + image.width(), " ", image.height(), "\n255\n");
std::to_string(image.height()) + "\n255\n";
file_writer->Write(header.data(), header.size()); file_writer->Write(header.data(), header.size());
for (int y = 0; y < image.height(); ++y) { for (int y = 0; y < image.height(); ++y) {
for (int x = 0; x < image.width(); ++x) { 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) { ::cartographer::io::FileWriter* file_writer) {
// Magic constants taken directly from ros map_saver code: // Magic constants taken directly from ros map_saver code:
// https://github.com/ros-planning/navigation/blob/ac41d2480c4cf1602daf39a6e9629142731d92b0/map_server/src/map_saver.cpp#L114 // https://github.com/ros-planning/navigation/blob/ac41d2480c4cf1602daf39a6e9629142731d92b0/map_server/src/map_saver.cpp#L114
const std::string output = const std::string output = absl::StrCat(
"image: " + pgm_filename + "\n" + "image: ", pgm_filename, "\n", "resolution: ", resolution, "\n",
"resolution: " + std::to_string(resolution) + "\n" + "origin: [" + "origin: [", origin.x(), ", ", origin.y(),
std::to_string(origin.x()) + ", " + std::to_string(origin.y()) + ", 0.0]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n");
", 0.0]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196\n";
file_writer->Write(output.data(), output.size()); file_writer->Write(output.data(), output.size());
} }

16
cartographer_rviz/cartographer_rviz/ogre_slice.cc
View File

@ -24,6 +24,7 @@
#include "OgreMaterialManager.h" #include "OgreMaterialManager.h"
#include "OgreTechnique.h" #include "OgreTechnique.h"
#include "OgreTextureManager.h" #include "OgreTextureManager.h"
#include "absl/strings/str_cat.h"
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
namespace cartographer_rviz { namespace cartographer_rviz {
@ -37,9 +38,8 @@ constexpr char kSubmapTexturePrefix[] = "SubmapTexture";
std::string GetSliceIdentifier( std::string GetSliceIdentifier(
const ::cartographer::mapping::SubmapId& submap_id, const int slice_id) { const ::cartographer::mapping::SubmapId& submap_id, const int slice_id) {
return std::to_string(submap_id.trajectory_id) + "-" + return absl::StrCat(submap_id.trajectory_id, "-", submap_id.submap_index, "-",
std::to_string(submap_id.submap_index) + "-" + slice_id);
std::to_string(slice_id);
} }
} // namespace } // namespace
@ -60,12 +60,12 @@ OgreSlice::OgreSlice(const ::cartographer::mapping::SubmapId& id, int slice_id,
scene_manager_(scene_manager), scene_manager_(scene_manager),
submap_node_(submap_node), submap_node_(submap_node),
slice_node_(submap_node_->createChildSceneNode()), slice_node_(submap_node_->createChildSceneNode()),
manual_object_(scene_manager_->createManualObject( manual_object_(scene_manager_->createManualObject(absl::StrCat(
kManualObjectPrefix + GetSliceIdentifier(id, slice_id))) { kManualObjectPrefix, GetSliceIdentifier(id, slice_id)))) {
material_ = Ogre::MaterialManager::getSingleton().getByName( material_ = Ogre::MaterialManager::getSingleton().getByName(
kSubmapSourceMaterialName); kSubmapSourceMaterialName);
material_ = material_->clone(kSubmapMaterialPrefix + material_ = material_->clone(
GetSliceIdentifier(id_, slice_id_)); absl::StrCat(kSubmapMaterialPrefix, GetSliceIdentifier(id_, slice_id_)));
material_->setReceiveShadows(false); material_->setReceiveShadows(false);
material_->getTechnique(0)->setLightingEnabled(false); material_->getTechnique(0)->setLightingEnabled(false);
material_->setCullingMode(Ogre::CULL_NONE); material_->setCullingMode(Ogre::CULL_NONE);
@ -126,7 +126,7 @@ void OgreSlice::Update(
texture_.setNull(); texture_.setNull();
} }
const std::string texture_name = const std::string texture_name =
kSubmapTexturePrefix + GetSliceIdentifier(id_, slice_id_); absl::StrCat(kSubmapTexturePrefix, GetSliceIdentifier(id_, slice_id_));
texture_ = Ogre::TextureManager::getSingleton().loadRawData( texture_ = Ogre::TextureManager::getSingleton().loadRawData(
texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
pixel_stream, submap_texture.width, submap_texture.height, pixel_stream, submap_texture.width, submap_texture.height,