Skip to content

Commit

Permalink
Thread safety:
Browse files Browse the repository at this point in the history 
 * Keep same smart pointer to object within a scope.
  • Loading branch information
Leif Terry committed Oct 11, 2023
1 parent c11cecb commit 98e2f8d
Show file tree
Hide file tree
Showing 3 changed files with 40 additions and 33 deletions.
19 changes: 11 additions & 8 deletions nav2_collision_monitor/src/pointcloud.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#include "nav2_collision_monitor/pointcloud.hpp"

#include <functional>
#include <memory>

#include "sensor_msgs/point_cloud2_iterator.hpp"

Expand Down Expand Up @@ -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;
}

Expand All @@ -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))
{
Expand All @@ -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<float> iter_x(*data_, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*data_, "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_z(*data_, "z");
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*data_in, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*data_in, "y");
sensor_msgs::PointCloud2ConstIterator<float> 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) {
Expand Down Expand Up @@ -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<sensor_msgs::msg::PointCloud2>(*msg);
std::atomic_store(&data_, msg_copy);
}

} // namespace nav2_collision_monitor
30 changes: 16 additions & 14 deletions nav2_collision_monitor/src/range.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand All @@ -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))
{
Expand All @@ -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;
Expand All @@ -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;

Expand All @@ -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;

Expand All @@ -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<sensor_msgs::msg::Range>(*msg);
std::atomic_store(&data_, msg_copy);
}

} // namespace nav2_collision_monitor
24 changes: 13 additions & 11 deletions nav2_collision_monitor/src/scan.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand All @@ -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))
{
Expand All @@ -95,34 +96,35 @@ 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;
}
}

// 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<sensor_msgs::msg::LaserScan>(*msg);
std::atomic_store(&data_, msg_copy);
}

} // namespace nav2_collision_monitor

0 comments on commit 98e2f8d

Please sign in to comment.