From 98e2f8de99a289648a77ba5c094d95b0e2dbd53c Mon Sep 17 00:00:00 2001 From: Leif Terry Date: Thu, 28 Sep 2023 15:12:52 -0400 Subject: [PATCH] Thread safety: * Keep same smart pointer to object within a scope. --- nav2_collision_monitor/src/pointcloud.cpp | 19 ++++++++------ nav2_collision_monitor/src/range.cpp | 30 ++++++++++++----------- nav2_collision_monitor/src/scan.cpp | 24 +++++++++--------- 3 files changed, 40 insertions(+), 33 deletions(-) diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index f23f5e0bfe..b38c58b531 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -15,6 +15,7 @@ #include "nav2_collision_monitor/pointcloud.hpp" #include +#include #include "sensor_msgs/point_cloud2_iterator.hpp" @@ -71,10 +72,11 @@ void PointCloud::getData( { // Ignore data from the source if it is not being published yet or // not published for a long time - if (data_ == nullptr) { + auto data_in = std::atomic_load(&data_); + if (data_in == nullptr) { return; } - if (!sourceValid(data_->header.stamp, curr_time)) { + if (!sourceValid(data_in->header.stamp, curr_time)) { return; } @@ -84,7 +86,7 @@ void PointCloud::getData( // to the base frame and current time if ( !nav2_util::getTransform( - data_->header.frame_id, data_->header.stamp, + data_in->header.frame_id, data_in->header.stamp, base_frame_id_, curr_time, global_frame_id_, transform_tolerance_, tf_buffer_, tf_transform)) { @@ -96,16 +98,16 @@ void PointCloud::getData( // frames. if ( !nav2_util::getTransform( - data_->header.frame_id, base_frame_id_, + data_in->header.frame_id, base_frame_id_, transform_tolerance_, tf_buffer_, tf_transform)) { return; } } - sensor_msgs::PointCloud2ConstIterator iter_x(*data_, "x"); - sensor_msgs::PointCloud2ConstIterator iter_y(*data_, "y"); - sensor_msgs::PointCloud2ConstIterator iter_z(*data_, "z"); + sensor_msgs::PointCloud2ConstIterator iter_x(*data_in, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*data_in, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z(*data_in, "z"); // Refill data array with PointCloud points in base frame for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { @@ -139,7 +141,8 @@ void PointCloud::getParameters(std::string & source_topic) void PointCloud::dataCallback(sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { - data_ = msg; + sensor_msgs::msg::PointCloud2::ConstSharedPtr msg_copy = std::make_shared(*msg); + std::atomic_store(&data_, msg_copy); } } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index 56c0a4750f..296e6b2906 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -71,19 +71,20 @@ void Range::getData( { // Ignore data from the source if it is not being published yet or // not being published for a long time - if (data_ == nullptr) { + auto data_in = std::atomic_load(&data_); + if (data_in == nullptr) { return; } - if (!sourceValid(data_->header.stamp, curr_time)) { + if (!sourceValid(data_in->header.stamp, curr_time)) { return; } // Ignore data, if its range is out of scope of range sensor abilities - if (data_->range < data_->min_range || data_->range > data_->max_range) { + if (data_in->range < data_in->min_range || data_in->range > data_in->max_range) { RCLCPP_DEBUG( logger_, "[%s]: Data range %fm is out of {%f..%f} sensor span. Ignoring...", - source_name_.c_str(), data_->range, data_->min_range, data_->max_range); + source_name_.c_str(), data_in->range, data_in->min_range, data_in->max_range); return; } @@ -93,7 +94,7 @@ void Range::getData( // to the base frame and current time if ( !nav2_util::getTransform( - data_->header.frame_id, data_->header.stamp, + data_in->header.frame_id, data_in->header.stamp, base_frame_id_, curr_time, global_frame_id_, transform_tolerance_, tf_buffer_, tf_transform)) { @@ -105,7 +106,7 @@ void Range::getData( // frames. if ( !nav2_util::getTransform( - data_->header.frame_id, base_frame_id_, + data_in->header.frame_id, base_frame_id_, transform_tolerance_, tf_buffer_, tf_transform)) { return; @@ -115,14 +116,14 @@ void Range::getData( // Calculate poses and refill data array float angle; for ( - angle = -data_->field_of_view / 2; - angle < data_->field_of_view / 2; + angle = -data_in->field_of_view / 2; + angle < data_in->field_of_view / 2; angle += obstacles_angle_) { // Transform point coordinates from source frame -> to base frame tf2::Vector3 p_v3_s( - data_->range * std::cos(angle), - data_->range * std::sin(angle), + data_in->range * std::cos(angle), + data_in->range * std::sin(angle), 0.0); tf2::Vector3 p_v3_b = tf_transform * p_v3_s; @@ -131,12 +132,12 @@ void Range::getData( } // Make sure that last (field_of_view / 2) point will be in the data array - angle = data_->field_of_view / 2; + angle = data_in->field_of_view / 2; // Transform point coordinates from source frame -> to base frame tf2::Vector3 p_v3_s( - data_->range * std::cos(angle), - data_->range * std::sin(angle), + data_in->range * std::cos(angle), + data_in->range * std::sin(angle), 0.0); tf2::Vector3 p_v3_b = tf_transform * p_v3_s; @@ -160,7 +161,8 @@ void Range::getParameters(std::string & source_topic) void Range::dataCallback(sensor_msgs::msg::Range::ConstSharedPtr msg) { - data_ = msg; + sensor_msgs::msg::Range::ConstSharedPtr msg_copy = std::make_shared(*msg); + std::atomic_store(&data_, msg_copy); } } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index 603e2f04e6..fcfe15dd6c 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -70,10 +70,11 @@ void Scan::getData( { // Ignore data from the source if it is not being published yet or // not being published for a long time - if (data_ == nullptr) { + auto data_in = std::atomic_load(&data_); + if (data_in == nullptr) { return; } - if (!sourceValid(data_->header.stamp, curr_time)) { + if (!sourceValid(data_in->header.stamp, curr_time)) { return; } @@ -83,7 +84,7 @@ void Scan::getData( // to the base frame and current time if ( !nav2_util::getTransform( - data_->header.frame_id, data_->header.stamp, + data_in->header.frame_id, data_in->header.stamp, base_frame_id_, curr_time, global_frame_id_, transform_tolerance_, tf_buffer_, tf_transform)) { @@ -95,7 +96,7 @@ void Scan::getData( // frames. if ( !nav2_util::getTransform( - data_->header.frame_id, base_frame_id_, + data_in->header.frame_id, base_frame_id_, transform_tolerance_, tf_buffer_, tf_transform)) { return; @@ -103,26 +104,27 @@ void Scan::getData( } // Calculate poses and refill data array - float angle = data_->angle_min; - for (size_t i = 0; i < data_->ranges.size(); i++) { - if (data_->ranges[i] >= data_->range_min && data_->ranges[i] <= data_->range_max) { + float angle = data_in->angle_min; + for (size_t i = 0; i < data_in->ranges.size(); i++) { + if (data_in->ranges[i] >= data_in->range_min && data_in->ranges[i] <= data_in->range_max) { // Transform point coordinates from source frame -> to base frame tf2::Vector3 p_v3_s( - data_->ranges[i] * std::cos(angle), - data_->ranges[i] * std::sin(angle), + data_in->ranges[i] * std::cos(angle), + data_in->ranges[i] * std::sin(angle), 0.0); tf2::Vector3 p_v3_b = tf_transform * p_v3_s; // Refill data array data.push_back({p_v3_b.x(), p_v3_b.y()}); } - angle += data_->angle_increment; + angle += data_in->angle_increment; } } void Scan::dataCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr msg) { - data_ = msg; + sensor_msgs::msg::LaserScan::ConstSharedPtr msg_copy = std::make_shared(*msg); + std::atomic_store(&data_, msg_copy); } } // namespace nav2_collision_monitor