From ccf77d55e26634348184f87f90ba1a0f55e35b8b Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 30 Sep 2016 10:20:11 +0200 Subject: [PATCH] Move the ScopedRosLogSink to a separate file. (#74) --- cartographer_ros/CMakeLists.txt | 2 + .../src/cartographer_node_main.cc | 54 +------------ cartographer_ros/src/node_options.h | 6 +- cartographer_ros/src/occupancy_grid.h | 6 +- cartographer_ros/src/ros_log_sink.cc | 76 +++++++++++++++++++ cartographer_ros/src/ros_log_sink.h | 45 +++++++++++ cartographer_ros/src/sensor_data.h | 6 +- cartographer_ros/src/sensor_data_producer.h | 6 +- 8 files changed, 138 insertions(+), 63 deletions(-) create mode 100644 cartographer_ros/src/ros_log_sink.cc create mode 100644 cartographer_ros/src/ros_log_sink.h diff --git a/cartographer_ros/CMakeLists.txt b/cartographer_ros/CMakeLists.txt index 750ca82..2eeb30b 100644 --- a/cartographer_ros/CMakeLists.txt +++ b/cartographer_ros/CMakeLists.txt @@ -87,6 +87,8 @@ add_executable(cartographer_node src/node_options.h src/occupancy_grid.cc src/occupancy_grid.h + src/ros_log_sink.cc + src/ros_log_sink.h src/sensor_data.cc src/sensor_data.h src/sensor_data_producer.cc diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc index 4ef3cc2..792296d 100644 --- a/cartographer_ros/src/cartographer_node_main.cc +++ b/cartographer_ros/src/cartographer_node_main.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include +#include #include #include #include @@ -51,7 +51,6 @@ #include "cartographer_ros_msgs/SubmapQuery.h" #include "cartographer_ros_msgs/TrajectorySubmapList.h" #include "gflags/gflags.h" -#include "glog/log_severity.h" #include "glog/logging.h" #include "nav_msgs/OccupancyGrid.h" #include "nav_msgs/Odometry.h" @@ -65,6 +64,7 @@ #include "msg_conversion.h" #include "node_options.h" #include "occupancy_grid.h" +#include "ros_log_sink.h" #include "sensor_data.h" #include "sensor_data_producer.h" #include "time_conversion.h" @@ -535,7 +535,7 @@ void Node::PublishPoseAndScanMatchedPointCloud( void Node::SpinOccupancyGridThreadForever() { for (;;) { - std::this_thread::sleep_for(carto::common::FromMilliseconds(1000)); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); { carto::common::MutexLocker lock(&mutex_); if (terminating_) { @@ -615,54 +615,6 @@ void Run() { node.SpinForever(); } -const char* GetBasename(const char* filepath) { - const char* base = strrchr(filepath, '/'); - return base ? (base + 1) : filepath; -} - -// Makes Google logging use ROS logging for output while an instance of this -// class exists. -class ScopedRosLogSink : public google::LogSink { - public: - ScopedRosLogSink() : will_die_(false) { AddLogSink(this); } - ~ScopedRosLogSink() override { RemoveLogSink(this); } - - void send(google::LogSeverity severity, const char* filename, - const char* base_filename, int line, const struct ::tm* tm_time, - const char* message, size_t message_len) override { - const std::string message_string = google::LogSink::ToString( - severity, GetBasename(filename), line, tm_time, message, message_len); - switch (severity) { - case google::GLOG_INFO: - ROS_INFO_STREAM(message_string); - break; - - case google::GLOG_WARNING: - ROS_WARN_STREAM(message_string); - break; - - case google::GLOG_ERROR: - ROS_ERROR_STREAM(message_string); - break; - - case google::GLOG_FATAL: - ROS_FATAL_STREAM(message_string); - will_die_ = true; - break; - } - } - - void WaitTillSent() override { - if (will_die_) { - // Give ROS some time to actually publish our message. - std::this_thread::sleep_for(carto::common::FromMilliseconds(1000)); - } - } - - private: - bool will_die_; -}; - } // namespace } // namespace cartographer_ros diff --git a/cartographer_ros/src/node_options.h b/cartographer_ros/src/node_options.h index 382ce31..bc8bc08 100644 --- a/cartographer_ros/src/node_options.h +++ b/cartographer_ros/src/node_options.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAHPER_ROS_NODE_OPTIONS_H_ -#define CARTOGRAHPER_ROS_NODE_OPTIONS_H_ +#ifndef CARTOGRAPHER_ROS_NODE_OPTIONS_H_ +#define CARTOGRAPHER_ROS_NODE_OPTIONS_H_ #include @@ -53,4 +53,4 @@ NodeOptions CreateNodeOptions( } // namespace cartographer_ros -#endif // CARTOGRAHPER_ROS_NODE_OPTIONS_H_ +#endif // CARTOGRAPHER_ROS_NODE_OPTIONS_H_ diff --git a/cartographer_ros/src/occupancy_grid.h b/cartographer_ros/src/occupancy_grid.h index 66482ed..c44a773 100644 --- a/cartographer_ros/src/occupancy_grid.h +++ b/cartographer_ros/src/occupancy_grid.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAHPER_ROS_OCCUPANCY_GRID_H_ -#define CARTOGRAHPER_ROS_OCCUPANCY_GRID_H_ +#ifndef CARTOGRAPHER_ROS_OCCUPANCY_GRID_H_ +#define CARTOGRAPHER_ROS_OCCUPANCY_GRID_H_ #include @@ -32,4 +32,4 @@ void BuildOccupancyGrid( } // namespace cartographer_ros -#endif // CARTOGRAHPER_ROS_OCCUPANCY_GRID_H_ +#endif // CARTOGRAPHER_ROS_OCCUPANCY_GRID_H_ diff --git a/cartographer_ros/src/ros_log_sink.cc b/cartographer_ros/src/ros_log_sink.cc new file mode 100644 index 0000000..5e4c6c5 --- /dev/null +++ b/cartographer_ros/src/ros_log_sink.cc @@ -0,0 +1,76 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "ros_log_sink.h" + +#include +#include +#include +#include + +#include "glog/log_severity.h" +#include "ros/console.h" + +namespace cartographer_ros { + +namespace { + +const char* GetBasename(const char* filepath) { + const char* base = std::strrchr(filepath, '/'); + return base ? (base + 1) : filepath; +} + +} // namespace + +ScopedRosLogSink::ScopedRosLogSink() : will_die_(false) { AddLogSink(this); } +ScopedRosLogSink::~ScopedRosLogSink() { RemoveLogSink(this); } + +void ScopedRosLogSink::send(const ::google::LogSeverity severity, + const char* const filename, + const char* const base_filename, const int line, + const struct std::tm* const tm_time, + const char* const message, + const size_t message_len) { + const std::string message_string = ::google::LogSink::ToString( + severity, GetBasename(filename), line, tm_time, message, message_len); + switch (severity) { + case ::google::GLOG_INFO: + ROS_INFO_STREAM(message_string); + break; + + case ::google::GLOG_WARNING: + ROS_WARN_STREAM(message_string); + break; + + case ::google::GLOG_ERROR: + ROS_ERROR_STREAM(message_string); + break; + + case ::google::GLOG_FATAL: + ROS_FATAL_STREAM(message_string); + will_die_ = true; + break; + } +} + +void ScopedRosLogSink::WaitTillSent() { + if (will_die_) { + // Give ROS some time to actually publish our message. + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } +} + +} // namespace cartographer_ros diff --git a/cartographer_ros/src/ros_log_sink.h b/cartographer_ros/src/ros_log_sink.h new file mode 100644 index 0000000..deffc2b --- /dev/null +++ b/cartographer_ros/src/ros_log_sink.h @@ -0,0 +1,45 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_ROS_ROS_LOG_SINK_H_ +#define CARTOGRAPHER_ROS_ROS_LOG_SINK_H_ + +#include + +#include "glog/logging.h" + +namespace cartographer_ros { + +// Makes Google logging use ROS logging for output while an instance of this +// class exists. +class ScopedRosLogSink : public ::google::LogSink { + public: + ScopedRosLogSink(); + ~ScopedRosLogSink() override; + + void send(::google::LogSeverity severity, const char* filename, + const char* base_filename, int line, const struct std::tm* tm_time, + const char* message, size_t message_len) override; + + void WaitTillSent() override; + + private: + bool will_die_; +}; + +} // namespace cartographer_ros + +#endif // CARTOGRAPHER_ROS_ROS_LOG_SINK_H_ diff --git a/cartographer_ros/src/sensor_data.h b/cartographer_ros/src/sensor_data.h index c0a7527..5651af7 100644 --- a/cartographer_ros/src/sensor_data.h +++ b/cartographer_ros/src/sensor_data.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAHPER_ROS_SENSOR_DATA_H_ -#define CARTOGRAHPER_ROS_SENSOR_DATA_H_ +#ifndef CARTOGRAPHER_ROS_SENSOR_DATA_H_ +#define CARTOGRAPHER_ROS_SENSOR_DATA_H_ #include @@ -51,4 +51,4 @@ struct SensorData { } // namespace cartographer_ros -#endif // CARTOGRAHPER_ROS_SENSOR_DATA_H_ +#endif // CARTOGRAPHER_ROS_SENSOR_DATA_H_ diff --git a/cartographer_ros/src/sensor_data_producer.h b/cartographer_ros/src/sensor_data_producer.h index a3a31bf..bc9130f 100644 --- a/cartographer_ros/src/sensor_data_producer.h +++ b/cartographer_ros/src/sensor_data_producer.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAHPER_ROS_SENSOR_DATA_PRODUCER_H_ -#define CARTOGRAHPER_ROS_SENSOR_DATA_PRODUCER_H_ +#ifndef CARTOGRAPHER_ROS_SENSOR_DATA_PRODUCER_H_ +#define CARTOGRAPHER_ROS_SENSOR_DATA_PRODUCER_H_ #include "cartographer/mapping/sensor_collator.h" #include "geometry_msgs/Transform.h" @@ -60,4 +60,4 @@ class SensorDataProducer { } // namespace cartographer_ros -#endif // CARTOGRAHPER_ROS_SENSOR_DATA_PRODUCER_H_ +#endif // CARTOGRAPHER_ROS_SENSOR_DATA_PRODUCER_H_