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

[ABSL] Use absl::Mutex. (#969)

master
Alexander Belyaev 2018-08-07 11:05:42 +02:00 committed by GitHub
parent edf25139a2
commit 41c74de649
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
12 changed files with 65 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -230,7 +230,7 @@ MapBuilderBridge::GetLocalTrajectoryData() {
std::shared_ptr<const LocalTrajectoryData::LocalSlamData> local_slam_data; std::shared_ptr<const LocalTrajectoryData::LocalSlamData> local_slam_data;
{ {
cartographer::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
if (local_slam_data_.count(trajectory_id) == 0) { if (local_slam_data_.count(trajectory_id) == 0) {
continue; continue;
} }
@ -499,7 +499,7 @@ void MapBuilderBridge::OnLocalSlamResult(
std::make_shared<LocalTrajectoryData::LocalSlamData>( std::make_shared<LocalTrajectoryData::LocalSlamData>(
LocalTrajectoryData::LocalSlamData{time, local_pose, LocalTrajectoryData::LocalSlamData{time, local_pose,
std::move(range_data_in_local)}); std::move(range_data_in_local)});
cartographer::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
local_slam_data_[trajectory_id] = std::move(local_slam_data); local_slam_data_[trajectory_id] = std::move(local_slam_data);
} }

8
cartographer_ros/cartographer_ros/map_builder_bridge.h
View File

@ -22,7 +22,7 @@
#include <string> #include <string>
#include <unordered_map> #include <unordered_map>
#include "cartographer/common/mutex.h" #include "absl/synchronization/mutex.h"
#include "cartographer/mapping/map_builder_interface.h" #include "cartographer/mapping/map_builder_interface.h"
#include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
@ -84,7 +84,7 @@ class MapBuilderBridge {
GetTrajectoryStates(); GetTrajectoryStates();
cartographer_ros_msgs::SubmapList GetSubmapList(); cartographer_ros_msgs::SubmapList GetSubmapList();
std::unordered_map<int, LocalTrajectoryData> GetLocalTrajectoryData() std::unordered_map<int, LocalTrajectoryData> GetLocalTrajectoryData()
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
visualization_msgs::MarkerArray GetTrajectoryNodeList(); visualization_msgs::MarkerArray GetTrajectoryNodeList();
visualization_msgs::MarkerArray GetLandmarkPosesList(); visualization_msgs::MarkerArray GetLandmarkPosesList();
visualization_msgs::MarkerArray GetConstraintList(); visualization_msgs::MarkerArray GetConstraintList();
@ -96,9 +96,9 @@ class MapBuilderBridge {
const ::cartographer::common::Time time, const ::cartographer::common::Time time,
const ::cartographer::transform::Rigid3d local_pose, const ::cartographer::transform::Rigid3d local_pose,
::cartographer::sensor::RangeData range_data_in_local) ::cartographer::sensor::RangeData range_data_in_local)
EXCLUDES(mutex_); LOCKS_EXCLUDED(mutex_);
cartographer::common::Mutex mutex_; absl::Mutex mutex_;
const NodeOptions node_options_; const NodeOptions node_options_;
std::unordered_map<int, std::unordered_map<int,
std::shared_ptr<const LocalTrajectoryData::LocalSlamData>> std::shared_ptr<const LocalTrajectoryData::LocalSlamData>>

10
cartographer_ros/cartographer_ros/metrics/internal/gauge.h
View File

@ -20,7 +20,7 @@
#include <map> #include <map>
#include <string> #include <string>
#include "cartographer/common/mutex.h" #include "absl/synchronization/mutex.h"
#include "cartographer/metrics/gauge.h" #include "cartographer/metrics/gauge.h"
#include "cartographer_ros_msgs/Metric.h" #include "cartographer_ros_msgs/Metric.h"
@ -41,12 +41,12 @@ class Gauge : public ::cartographer::metrics::Gauge {
void Increment() override { Increment(1.); } void Increment() override { Increment(1.); }
void Set(double value) override { void Set(double value) override {
::cartographer::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
value_ = value; value_ = value;
} }
double Value() { double Value() {
::cartographer::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
return value_; return value_;
} }
@ -65,11 +65,11 @@ class Gauge : public ::cartographer::metrics::Gauge {
private: private:
void Add(const double value) { void Add(const double value) {
::cartographer::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
value_ += value; value_ += value;
} }
cartographer::common::Mutex mutex_; absl::Mutex mutex_;
const std::map<std::string, std::string> labels_; const std::map<std::string, std::string> labels_;
double value_ GUARDED_BY(mutex_); double value_ GUARDED_BY(mutex_);
}; };

10
cartographer_ros/cartographer_ros/metrics/internal/histogram.cc
View File

@ -31,7 +31,7 @@ Histogram::Histogram(const std::map<std::string, std::string>& labels,
: labels_(labels), : labels_(labels),
bucket_boundaries_(bucket_boundaries), bucket_boundaries_(bucket_boundaries),
bucket_counts_(bucket_boundaries.size() + 1) { bucket_counts_(bucket_boundaries.size() + 1) {
::cartographer::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
CHECK(std::is_sorted(std::begin(bucket_boundaries_), CHECK(std::is_sorted(std::begin(bucket_boundaries_),
std::end(bucket_boundaries_))); std::end(bucket_boundaries_)));
} }
@ -41,13 +41,13 @@ void Histogram::Observe(double value) {
std::distance(bucket_boundaries_.begin(), std::distance(bucket_boundaries_.begin(),
std::upper_bound(bucket_boundaries_.begin(), std::upper_bound(bucket_boundaries_.begin(),
bucket_boundaries_.end(), value)); bucket_boundaries_.end(), value));
::cartographer::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
sum_ += value; sum_ += value;
bucket_counts_[bucket_index] += 1; bucket_counts_[bucket_index] += 1;
} }
std::map<double, double> Histogram::CountsByBucket() { std::map<double, double> Histogram::CountsByBucket() {
::cartographer::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
std::map<double, double> counts_by_bucket; std::map<double, double> counts_by_bucket;
// Add the finite buckets. // Add the finite buckets.
for (size_t i = 0; i < bucket_boundaries_.size(); ++i) { for (size_t i = 0; i < bucket_boundaries_.size(); ++i) {
@ -59,12 +59,12 @@ std::map<double, double> Histogram::CountsByBucket() {
} }
double Histogram::Sum() { double Histogram::Sum() {
::cartographer::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
return sum_; return sum_;
} }
double Histogram::CumulativeCount() { double Histogram::CumulativeCount() {
::cartographer::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
return std::accumulate(bucket_counts_.begin(), bucket_counts_.end(), 0.); return std::accumulate(bucket_counts_.begin(), bucket_counts_.end(), 0.);
} }

4
cartographer_ros/cartographer_ros/metrics/internal/histogram.h
View File

@ -20,7 +20,7 @@
#include <map> #include <map>
#include <vector> #include <vector>
#include "cartographer/common/mutex.h" #include "absl/synchronization/mutex.h"
#include "cartographer/metrics/histogram.h" #include "cartographer/metrics/histogram.h"
#include "cartographer_ros_msgs/Metric.h" #include "cartographer_ros_msgs/Metric.h"
@ -47,7 +47,7 @@ class Histogram : public ::cartographer::metrics::Histogram {
cartographer_ros_msgs::Metric ToRosMessage(); cartographer_ros_msgs::Metric ToRosMessage();
private: private:
cartographer::common::Mutex mutex_; absl::Mutex mutex_;
const std::map<std::string, std::string> labels_; const std::map<std::string, std::string> labels_;
const BucketBoundaries bucket_boundaries_; const BucketBoundaries bucket_boundaries_;
std::vector<double> bucket_counts_ GUARDED_BY(mutex_); std::vector<double> bucket_counts_ GUARDED_BY(mutex_);

50
cartographer_ros/cartographer_ros/node.cc
View File

@ -93,7 +93,7 @@ Node::Node(
tf2_ros::Buffer* const tf_buffer, const bool collect_metrics) tf2_ros::Buffer* const tf_buffer, const bool collect_metrics)
: node_options_(node_options), : node_options_(node_options),
map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) { map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) {
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
if (collect_metrics) { if (collect_metrics) {
metrics_registry_ = absl::make_unique<metrics::FamilyFactory>(); metrics_registry_ = absl::make_unique<metrics::FamilyFactory>();
carto::metrics::RegisterAllMetrics(metrics_registry_.get()); carto::metrics::RegisterAllMetrics(metrics_registry_.get());
@ -154,13 +154,13 @@ Node::~Node() { FinishAllTrajectories(); }
bool Node::HandleSubmapQuery( bool Node::HandleSubmapQuery(
::cartographer_ros_msgs::SubmapQuery::Request& request, ::cartographer_ros_msgs::SubmapQuery::Request& request,
::cartographer_ros_msgs::SubmapQuery::Response& response) { ::cartographer_ros_msgs::SubmapQuery::Response& response) {
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
map_builder_bridge_.HandleSubmapQuery(request, response); map_builder_bridge_.HandleSubmapQuery(request, response);
return true; return true;
} }
void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) { void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) {
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList()); submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList());
} }
@ -193,7 +193,7 @@ void Node::AddSensorSamplers(const int trajectory_id,
} }
void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) { void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) {
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
for (const auto& entry : map_builder_bridge_.GetLocalTrajectoryData()) { for (const auto& entry : map_builder_bridge_.GetLocalTrajectoryData()) {
const auto& trajectory_data = entry.second; const auto& trajectory_data = entry.second;
@ -285,7 +285,7 @@ void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) {
void Node::PublishTrajectoryNodeList( void Node::PublishTrajectoryNodeList(
const ::ros::WallTimerEvent& unused_timer_event) { const ::ros::WallTimerEvent& unused_timer_event) {
if (trajectory_node_list_publisher_.getNumSubscribers() > 0) { if (trajectory_node_list_publisher_.getNumSubscribers() > 0) {
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
trajectory_node_list_publisher_.publish( trajectory_node_list_publisher_.publish(
map_builder_bridge_.GetTrajectoryNodeList()); map_builder_bridge_.GetTrajectoryNodeList());
} }
@ -294,7 +294,7 @@ void Node::PublishTrajectoryNodeList(
void Node::PublishLandmarkPosesList( void Node::PublishLandmarkPosesList(
const ::ros::WallTimerEvent& unused_timer_event) { const ::ros::WallTimerEvent& unused_timer_event) {
if (landmark_poses_list_publisher_.getNumSubscribers() > 0) { if (landmark_poses_list_publisher_.getNumSubscribers() > 0) {
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
landmark_poses_list_publisher_.publish( landmark_poses_list_publisher_.publish(
map_builder_bridge_.GetLandmarkPosesList()); map_builder_bridge_.GetLandmarkPosesList());
} }
@ -303,7 +303,7 @@ void Node::PublishLandmarkPosesList(
void Node::PublishConstraintList( void Node::PublishConstraintList(
const ::ros::WallTimerEvent& unused_timer_event) { const ::ros::WallTimerEvent& unused_timer_event) {
if (constraint_list_publisher_.getNumSubscribers() > 0) { if (constraint_list_publisher_.getNumSubscribers() > 0) {
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
constraint_list_publisher_.publish(map_builder_bridge_.GetConstraintList()); constraint_list_publisher_.publish(map_builder_bridge_.GetConstraintList());
} }
} }
@ -535,7 +535,7 @@ cartographer_ros_msgs::StatusResponse Node::FinishTrajectoryUnderLock(
bool Node::HandleStartTrajectory( bool Node::HandleStartTrajectory(
::cartographer_ros_msgs::StartTrajectory::Request& request, ::cartographer_ros_msgs::StartTrajectory::Request& request,
::cartographer_ros_msgs::StartTrajectory::Response& response) { ::cartographer_ros_msgs::StartTrajectory::Response& response) {
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
TrajectoryOptions options; TrajectoryOptions options;
if (!FromRosMessage(request.options, &options) || if (!FromRosMessage(request.options, &options) ||
!ValidateTrajectoryOptions(options)) { !ValidateTrajectoryOptions(options)) {
@ -557,7 +557,7 @@ bool Node::HandleStartTrajectory(
} }
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) { void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
CHECK(ValidateTrajectoryOptions(options)); CHECK(ValidateTrajectoryOptions(options));
AddTrajectory(options, DefaultSensorTopics()); AddTrajectory(options, DefaultSensorTopics());
} }
@ -587,7 +587,7 @@ int Node::AddOfflineTrajectory(
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>& const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
expected_sensor_ids, expected_sensor_ids,
const TrajectoryOptions& options) { const TrajectoryOptions& options) {
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
const int trajectory_id = const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
AddExtrapolator(trajectory_id, options); AddExtrapolator(trajectory_id, options);
@ -600,7 +600,7 @@ bool Node::HandleGetTrajectoryStates(
::cartographer_ros_msgs::GetTrajectoryStates::Response& response) { ::cartographer_ros_msgs::GetTrajectoryStates::Response& response) {
using TrajectoryState = using TrajectoryState =
::cartographer::mapping::PoseGraphInterface::TrajectoryState; ::cartographer::mapping::PoseGraphInterface::TrajectoryState;
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
response.status.code = ::cartographer_ros_msgs::StatusCode::OK; response.status.code = ::cartographer_ros_msgs::StatusCode::OK;
response.trajectory_states.header.stamp = ros::Time::now(); response.trajectory_states.header.stamp = ros::Time::now();
for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) { for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
@ -630,7 +630,7 @@ bool Node::HandleGetTrajectoryStates(
bool Node::HandleFinishTrajectory( bool Node::HandleFinishTrajectory(
::cartographer_ros_msgs::FinishTrajectory::Request& request, ::cartographer_ros_msgs::FinishTrajectory::Request& request,
::cartographer_ros_msgs::FinishTrajectory::Response& response) { ::cartographer_ros_msgs::FinishTrajectory::Response& response) {
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
response.status = FinishTrajectoryUnderLock(request.trajectory_id); response.status = FinishTrajectoryUnderLock(request.trajectory_id);
return true; return true;
} }
@ -638,7 +638,7 @@ bool Node::HandleFinishTrajectory(
bool Node::HandleWriteState( bool Node::HandleWriteState(
::cartographer_ros_msgs::WriteState::Request& request, ::cartographer_ros_msgs::WriteState::Request& request,
::cartographer_ros_msgs::WriteState::Response& response) { ::cartographer_ros_msgs::WriteState::Response& response) {
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
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;
@ -653,7 +653,7 @@ bool Node::HandleWriteState(
bool Node::HandleReadMetrics( bool Node::HandleReadMetrics(
::cartographer_ros_msgs::ReadMetrics::Request& request, ::cartographer_ros_msgs::ReadMetrics::Request& request,
::cartographer_ros_msgs::ReadMetrics::Response& response) { ::cartographer_ros_msgs::ReadMetrics::Response& response) {
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
response.timestamp = ros::Time::now(); response.timestamp = ros::Time::now();
if (!metrics_registry_) { if (!metrics_registry_) {
response.status.code = cartographer_ros_msgs::StatusCode::UNAVAILABLE; response.status.code = cartographer_ros_msgs::StatusCode::UNAVAILABLE;
@ -667,7 +667,7 @@ bool Node::HandleReadMetrics(
} }
void Node::FinishAllTrajectories() { void Node::FinishAllTrajectories() {
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) { for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
if (entry.second == TrajectoryState::ACTIVE) { if (entry.second == TrajectoryState::ACTIVE) {
const int trajectory_id = entry.first; const int trajectory_id = entry.first;
@ -678,7 +678,7 @@ void Node::FinishAllTrajectories() {
} }
bool Node::FinishTrajectory(const int trajectory_id) { bool Node::FinishTrajectory(const int trajectory_id) {
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
return FinishTrajectoryUnderLock(trajectory_id).code == return FinishTrajectoryUnderLock(trajectory_id).code ==
cartographer_ros_msgs::StatusCode::OK; cartographer_ros_msgs::StatusCode::OK;
} }
@ -706,7 +706,7 @@ void Node::RunFinalOptimization() {
void Node::HandleOdometryMessage(const int trajectory_id, void Node::HandleOdometryMessage(const int trajectory_id,
const std::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_); absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) { if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) {
return; return;
} }
@ -721,7 +721,7 @@ void Node::HandleOdometryMessage(const int trajectory_id,
void Node::HandleNavSatFixMessage(const int trajectory_id, void Node::HandleNavSatFixMessage(const int trajectory_id,
const std::string& sensor_id, const std::string& sensor_id,
const sensor_msgs::NavSatFix::ConstPtr& msg) { const sensor_msgs::NavSatFix::ConstPtr& msg) {
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).fixed_frame_pose_sampler.Pulse()) { if (!sensor_samplers_.at(trajectory_id).fixed_frame_pose_sampler.Pulse()) {
return; return;
} }
@ -732,7 +732,7 @@ void Node::HandleNavSatFixMessage(const int trajectory_id,
void Node::HandleLandmarkMessage( void Node::HandleLandmarkMessage(
const int trajectory_id, const std::string& sensor_id, const int trajectory_id, const std::string& sensor_id,
const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) { const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) {
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) { if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) {
return; return;
} }
@ -743,7 +743,7 @@ void Node::HandleLandmarkMessage(
void Node::HandleImuMessage(const int trajectory_id, void Node::HandleImuMessage(const int trajectory_id,
const std::string& sensor_id, const std::string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg) { const sensor_msgs::Imu::ConstPtr& msg) {
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) { if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) {
return; return;
} }
@ -758,7 +758,7 @@ void Node::HandleImuMessage(const int trajectory_id,
void Node::HandleLaserScanMessage(const int trajectory_id, void Node::HandleLaserScanMessage(const int trajectory_id,
const std::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_); absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return; return;
} }
@ -769,7 +769,7 @@ void Node::HandleLaserScanMessage(const int trajectory_id,
void Node::HandleMultiEchoLaserScanMessage( void Node::HandleMultiEchoLaserScanMessage(
const int trajectory_id, const std::string& sensor_id, const 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_); absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return; return;
} }
@ -780,7 +780,7 @@ void Node::HandleMultiEchoLaserScanMessage(
void Node::HandlePointCloud2Message( void Node::HandlePointCloud2Message(
const int trajectory_id, const std::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_); absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return; return;
} }
@ -790,7 +790,7 @@ void Node::HandlePointCloud2Message(
void Node::SerializeState(const std::string& filename, void Node::SerializeState(const std::string& filename,
const bool include_unfinished_submaps) { const bool include_unfinished_submaps) {
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
CHECK( CHECK(
map_builder_bridge_.SerializeState(filename, include_unfinished_submaps)) map_builder_bridge_.SerializeState(filename, include_unfinished_submaps))
<< "Could not write state."; << "Could not write state.";
@ -798,7 +798,7 @@ void Node::SerializeState(const std::string& filename,
void Node::LoadState(const std::string& state_filename, void Node::LoadState(const std::string& state_filename,
const bool load_frozen_state) { const bool load_frozen_state) {
carto::common::MutexLocker lock(&mutex_); absl::MutexLock lock(&mutex_);
map_builder_bridge_.LoadState(state_filename, load_frozen_state); map_builder_bridge_.LoadState(state_filename, load_frozen_state);
} }

6
cartographer_ros/cartographer_ros/node.h
View File

@ -24,8 +24,8 @@
#include <unordered_set> #include <unordered_set>
#include <vector> #include <vector>
#include "absl/synchronization/mutex.h"
#include "cartographer/common/fixed_ratio_sampler.h" #include "cartographer/common/fixed_ratio_sampler.h"
#include "cartographer/common/mutex.h"
#include "cartographer/mapping/map_builder_interface.h" #include "cartographer/mapping/map_builder_interface.h"
#include "cartographer/mapping/pose_extrapolator.h" #include "cartographer/mapping/pose_extrapolator.h"
#include "cartographer_ros/map_builder_bridge.h" #include "cartographer_ros/map_builder_bridge.h"
@ -171,14 +171,14 @@ class Node {
bool ValidateTopicNames(const ::cartographer_ros_msgs::SensorTopics& topics, bool ValidateTopicNames(const ::cartographer_ros_msgs::SensorTopics& topics,
const TrajectoryOptions& options); const TrajectoryOptions& options);
cartographer_ros_msgs::StatusResponse FinishTrajectoryUnderLock( cartographer_ros_msgs::StatusResponse FinishTrajectoryUnderLock(
int trajectory_id) REQUIRES(mutex_); int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(mutex_);
void MaybeWarnAboutTopicMismatch(const ::ros::WallTimerEvent&); void MaybeWarnAboutTopicMismatch(const ::ros::WallTimerEvent&);
const NodeOptions node_options_; const NodeOptions node_options_;
tf2_ros::TransformBroadcaster tf_broadcaster_; tf2_ros::TransformBroadcaster tf_broadcaster_;
cartographer::common::Mutex mutex_; absl::Mutex mutex_;
MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_); MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);
::ros::NodeHandle node_handle_; ::ros::NodeHandle node_handle_;

8
cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc
View File

@ -20,8 +20,8 @@
#include "Eigen/Core" #include "Eigen/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "absl/synchronization/mutex.h"
#include "cairo/cairo.h" #include "cairo/cairo.h"
#include "cartographer/common/mutex.h"
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer/io/image.h" #include "cartographer/io/image.h"
#include "cartographer/io/submap_painter.h" #include "cartographer/io/submap_painter.h"
@ -72,7 +72,7 @@ class Node {
::ros::NodeHandle node_handle_; ::ros::NodeHandle node_handle_;
const double resolution_; const double resolution_;
::cartographer::common::Mutex mutex_; absl::Mutex mutex_;
::ros::ServiceClient client_ GUARDED_BY(mutex_); ::ros::ServiceClient client_ GUARDED_BY(mutex_);
::ros::Subscriber submap_list_subscriber_ GUARDED_BY(mutex_); ::ros::Subscriber submap_list_subscriber_ GUARDED_BY(mutex_);
::ros::Publisher occupancy_grid_publisher_ GUARDED_BY(mutex_); ::ros::Publisher occupancy_grid_publisher_ GUARDED_BY(mutex_);
@ -103,7 +103,7 @@ Node::Node(const double resolution, const double publish_period_sec)
void Node::HandleSubmapList( void Node::HandleSubmapList(
const cartographer_ros_msgs::SubmapList::ConstPtr& msg) { const cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
::cartographer::common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
// We do not do any work if nobody listens. // We do not do any work if nobody listens.
if (occupancy_grid_publisher_.getNumSubscribers() == 0) { if (occupancy_grid_publisher_.getNumSubscribers() == 0) {
@ -168,7 +168,7 @@ void Node::DrawAndPublish(const ::ros::WallTimerEvent& unused_timer_event) {
return; return;
} }
::cartographer::common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
auto painted_slices = PaintSubmapSlices(submap_slices_, resolution_); auto painted_slices = PaintSubmapSlices(submap_slices_, resolution_);
std::unique_ptr<nav_msgs::OccupancyGrid> msg_ptr = CreateOccupancyGridMsg( std::unique_ptr<nav_msgs::OccupancyGrid> msg_ptr = CreateOccupancyGridMsg(
painted_slices, resolution_, last_frame_id_, last_timestamp_); painted_slices, resolution_, last_frame_id_, last_timestamp_);

10
cartographer_rviz/cartographer_rviz/drawable_submap.cc
View File

@ -96,7 +96,7 @@ DrawableSubmap::~DrawableSubmap() {
void DrawableSubmap::Update( void DrawableSubmap::Update(
const ::std_msgs::Header& header, const ::std_msgs::Header& header,
const ::cartographer_ros_msgs::SubmapEntry& metadata) { const ::cartographer_ros_msgs::SubmapEntry& metadata) {
::cartographer::common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
metadata_version_ = metadata.submap_version; metadata_version_ = metadata.submap_version;
pose_ = ::cartographer_ros::ToRigid3d(metadata.pose); pose_ = ::cartographer_ros::ToRigid3d(metadata.pose);
submap_node_->setPosition(ToOgre(pose_.translation())); submap_node_->setPosition(ToOgre(pose_.translation()));
@ -113,7 +113,7 @@ void DrawableSubmap::Update(
} }
bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) { bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) {
::cartographer::common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
// Received metadata version can also be lower if we restarted Cartographer. // Received metadata version can also be lower if we restarted Cartographer.
const bool newer_version_available = const bool newer_version_available =
submap_textures_ == nullptr || submap_textures_ == nullptr ||
@ -131,7 +131,7 @@ bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) {
rpc_request_future_ = std::async(std::launch::async, [this, client]() { rpc_request_future_ = std::async(std::launch::async, [this, client]() {
std::unique_ptr<::cartographer::io::SubmapTextures> submap_textures = std::unique_ptr<::cartographer::io::SubmapTextures> submap_textures =
::cartographer_ros::FetchSubmapTextures(id_, client); ::cartographer_ros::FetchSubmapTextures(id_, client);
::cartographer::common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
query_in_progress_ = false; query_in_progress_ = false;
if (submap_textures != nullptr) { if (submap_textures != nullptr) {
// We emit a signal to update in the right thread, and pass via the // We emit a signal to update in the right thread, and pass via the
@ -145,7 +145,7 @@ bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) {
} }
bool DrawableSubmap::QueryInProgress() { bool DrawableSubmap::QueryInProgress() {
::cartographer::common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
return query_in_progress_; return query_in_progress_;
} }
@ -176,7 +176,7 @@ void DrawableSubmap::SetSliceVisibility(size_t slice_index, bool visible) {
} }
void DrawableSubmap::UpdateSceneNode() { void DrawableSubmap::UpdateSceneNode() {
::cartographer::common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
for (size_t slice_index = 0; slice_index < ogre_slices_.size() && for (size_t slice_index = 0; slice_index < ogre_slices_.size() &&
slice_index < submap_textures_->textures.size(); slice_index < submap_textures_->textures.size();
++slice_index) { ++slice_index) {

4
cartographer_rviz/cartographer_rviz/drawable_submap.h
View File

@ -24,7 +24,7 @@
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "OgreSceneManager.h" #include "OgreSceneManager.h"
#include "OgreSceneNode.h" #include "OgreSceneNode.h"
#include "cartographer/common/mutex.h" #include "absl/synchronization/mutex.h"
#include "cartographer/io/submap_painter.h" #include "cartographer/io/submap_painter.h"
#include "cartographer/mapping/id.h" #include "cartographer/mapping/id.h"
#include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/rigid_transform.h"
@ -96,7 +96,7 @@ class DrawableSubmap : public QObject {
private: private:
const ::cartographer::mapping::SubmapId id_; const ::cartographer::mapping::SubmapId id_;
::cartographer::common::Mutex mutex_; absl::Mutex mutex_;
::rviz::DisplayContext* const display_context_; ::rviz::DisplayContext* const display_context_;
Ogre::SceneNode* const submap_node_; Ogre::SceneNode* const submap_node_;
Ogre::SceneNode* const submap_id_text_node_; Ogre::SceneNode* const submap_id_text_node_;

12
cartographer_rviz/cartographer_rviz/submaps_display.cc
View File

@ -18,7 +18,7 @@
#include "OgreResourceGroupManager.h" #include "OgreResourceGroupManager.h"
#include "absl/memory/memory.h" #include "absl/memory/memory.h"
#include "cartographer/common/mutex.h" #include "absl/synchronization/mutex.h"
#include "cartographer/mapping/id.h" #include "cartographer/mapping/id.h"
#include "cartographer_ros_msgs/SubmapList.h" #include "cartographer_ros_msgs/SubmapList.h"
#include "cartographer_ros_msgs/SubmapQuery.h" #include "cartographer_ros_msgs/SubmapQuery.h"
@ -103,7 +103,7 @@ void SubmapsDisplay::onInitialize() {
void SubmapsDisplay::reset() { void SubmapsDisplay::reset() {
MFDClass::reset(); MFDClass::reset();
::cartographer::common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
client_.shutdown(); client_.shutdown();
trajectories_.clear(); trajectories_.clear();
CreateClient(); CreateClient();
@ -111,7 +111,7 @@ void SubmapsDisplay::reset() {
void SubmapsDisplay::processMessage( void SubmapsDisplay::processMessage(
const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) { const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
::cartographer::common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
map_frame_ = absl::make_unique<std::string>(msg->header.frame_id); map_frame_ = absl::make_unique<std::string>(msg->header.frame_id);
// In case Cartographer node is relaunched, destroy trajectories from the // In case Cartographer node is relaunched, destroy trajectories from the
// previous instance. // previous instance.
@ -181,7 +181,7 @@ void SubmapsDisplay::processMessage(
} }
void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
::cartographer::common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
// Schedule fetching of new submap textures. // Schedule fetching of new submap textures.
for (const auto& trajectory : trajectories_) { for (const auto& trajectory : trajectories_) {
int num_ongoing_requests = 0; int num_ongoing_requests = 0;
@ -230,7 +230,7 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
} }
void SubmapsDisplay::AllEnabledToggled() { void SubmapsDisplay::AllEnabledToggled() {
::cartographer::common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
const bool visible = visibility_all_enabled_->getBool(); const bool visible = visibility_all_enabled_->getBool();
for (auto& trajectory : trajectories_) { for (auto& trajectory : trajectories_) {
trajectory->visibility->setBool(visible); trajectory->visibility->setBool(visible);
@ -238,7 +238,7 @@ void SubmapsDisplay::AllEnabledToggled() {
} }
void SubmapsDisplay::ResolutionToggled() { void SubmapsDisplay::ResolutionToggled() {
::cartographer::common::MutexLocker locker(&mutex_); absl::MutexLock locker(&mutex_);
for (auto& trajectory : trajectories_) { for (auto& trajectory : trajectories_) {
for (auto& submap_entry : trajectory->submaps) { for (auto& submap_entry : trajectory->submaps) {
submap_entry.second->SetSliceVisibility( submap_entry.second->SetSliceVisibility(

4
cartographer_rviz/cartographer_rviz/submaps_display.h
View File

@ -22,7 +22,7 @@
#include <string> #include <string>
#include <vector> #include <vector>
#include "cartographer/common/mutex.h" #include "absl/synchronization/mutex.h"
#include "cartographer/common/port.h" #include "cartographer/common/port.h"
#include "cartographer_ros_msgs/SubmapList.h" #include "cartographer_ros_msgs/SubmapList.h"
#include "cartographer_rviz/drawable_submap.h" #include "cartographer_rviz/drawable_submap.h"
@ -90,7 +90,7 @@ class SubmapsDisplay
::rviz::StringProperty* tracking_frame_property_; ::rviz::StringProperty* tracking_frame_property_;
Ogre::SceneNode* map_node_ = nullptr; // Represents the map frame. Ogre::SceneNode* map_node_ = nullptr; // Represents the map frame.
std::vector<std::unique_ptr<Trajectory>> trajectories_ GUARDED_BY(mutex_); std::vector<std::unique_ptr<Trajectory>> trajectories_ GUARDED_BY(mutex_);
::cartographer::common::Mutex mutex_; absl::Mutex mutex_;
::rviz::BoolProperty* slice_high_resolution_enabled_; ::rviz::BoolProperty* slice_high_resolution_enabled_;
::rviz::BoolProperty* slice_low_resolution_enabled_; ::rviz::BoolProperty* slice_low_resolution_enabled_;
::rviz::Property* trajectories_category_; ::rviz::Property* trajectories_category_;