Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
polygon_source.cpp
1 // Copyright (c) 2023 Pixel Robotics GmbH
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #include "nav2_collision_monitor/polygon_source.hpp"
16 
17 #include <cmath>
18 #include <functional>
19 #include <unordered_map>
20 
21 #include "geometry_msgs/msg/polygon_stamped.hpp"
22 #include "tf2/transform_datatypes.hpp"
23 #include "nav2_ros_common/tf2_factories.hpp"
24 
25 #include "nav2_ros_common/node_utils.hpp"
26 #include "nav2_util/robot_utils.hpp"
27 
28 
29 namespace nav2_collision_monitor
30 {
31 
33  const nav2::LifecycleNode::WeakPtr & node,
34  const std::string & source_name,
35  const nav2::TransformBuffer::SharedPtr tf_buffer,
36  const std::string & base_frame_id,
37  const std::string & global_frame_id,
38  const tf2::Duration & transform_tolerance,
39  const rclcpp::Duration & source_timeout,
40  const bool base_shift_correction)
41 : Source(
42  node, source_name, tf_buffer, base_frame_id, global_frame_id,
43  transform_tolerance, source_timeout, base_shift_correction)
44 {
45 }
46 
48 {
49  data_sub_.reset();
50 }
51 
53 {
54  if (!Source::configure()) {
55  return false;
56  }
57  auto node = node_.lock();
58  if (!node) {
59  throw std::runtime_error{"Failed to lock node"};
60  }
61 
62  std::string source_topic;
63 
64  getParameters(source_topic);
65 
66  data_sub_ = node->create_subscription<geometry_msgs::msg::PolygonInstanceStamped>(
67  source_topic,
68  std::bind(&PolygonSource::dataCallback, this, std::placeholders::_1),
70 
71  return true;
72 }
73 
75  const rclcpp::Time & curr_time,
76  std::vector<Point> & data)
77 {
78  // Ignore data from the source if it is not being published yet or
79  // not published for a long time
80  if (data_.empty()) {
81  return false;
82  }
83 
84  // Remove stale data
85  data_.erase(
86  std::remove_if(
87  data_.begin(), data_.end(),
88  [this, curr_time](const geometry_msgs::msg::PolygonInstanceStamped & polygon_stamped) {
89  return curr_time - rclcpp::Time(polygon_stamped.header.stamp) > source_timeout_;
90  }), data_.end());
91 
92  // Reusing the same transform for each frame so polygons in that
93  // frame are transformed consistently during this processing cycle.
94  std::unordered_map<std::string, tf2::Transform> transforms;
95  for (const auto & polygon_instance : data_) {
96  tf2::Transform tf_transform;
98  if (!getTransform(curr_time, polygon_instance.header, tf_transform)) {
99  return false;
100  }
101  } else {
102  const auto [it, inserted] = transforms.try_emplace(polygon_instance.header.frame_id);
103  if (inserted && !getTransform(curr_time, polygon_instance.header, it->second)) {
104  return false;
105  }
106  tf_transform = it->second;
107  }
108  geometry_msgs::msg::PolygonStamped poly_out, polygon_stamped;
109  geometry_msgs::msg::TransformStamped tf;
110  tf.transform = tf2::toMsg(tf_transform);
111  polygon_stamped.header = polygon_instance.header;
112  polygon_stamped.polygon = polygon_instance.polygon.polygon;
113  tf2::doTransform(polygon_stamped, poly_out, tf);
114  convertPolygonStampedToPoints(poly_out, data);
115  }
116  return true;
117 }
118 
120  const geometry_msgs::msg::PolygonStamped & polygon, std::vector<Point> & data) const
121 {
122  /* Full function generated by GPT */
123 
124  // Iterate over the vertices of the polygon
125  for (size_t i = 0; i < polygon.polygon.points.size(); ++i) {
126  const auto & current_point = polygon.polygon.points[i];
127  const auto & next_point =
128  polygon.polygon.points[(i + 1) % polygon.polygon.points.size()];
129 
130  // Calculate the distance between the current and next points
131  double segment_length =
132  std::hypot(next_point.x - current_point.x, next_point.y - current_point.y);
133 
134  // Calculate the number of points to sample in the current segment
135  int num_points_in_segment =
136  std::max(static_cast<int>(std::ceil(segment_length / sampling_distance_)), 1);
137 
138  // Calculate the step size for each pair of vertices
139  const double dx = (next_point.x - current_point.x) / num_points_in_segment;
140  const double dy = (next_point.y - current_point.y) / num_points_in_segment;
141 
142  // Sample the points with equal spacing
143  for (int j = 0; j <= num_points_in_segment; ++j) {
144  Point p;
145  p.x = current_point.x + j * dx;
146  p.y = current_point.y + j * dy;
147  p.source = source_name_;
148  data.push_back(p);
149  }
150  }
151 }
152 
153 void PolygonSource::getParameters(std::string & source_topic)
154 {
155  auto node = node_.lock();
156  if (!node) {
157  throw std::runtime_error{"Failed to lock node"};
158  }
159 
160  getCommonParameters(source_topic);
161 
162  sampling_distance_ = node->declare_or_get_parameter(
163  source_name_ + ".sampling_distance", 0.1);
164 }
165 
166 void PolygonSource::dataCallback(geometry_msgs::msg::PolygonInstanceStamped::ConstSharedPtr msg)
167 {
168  auto node = node_.lock();
169  if (!node) {
170  throw std::runtime_error{"Failed to lock node"};
171  }
172  auto curr_time = node->now();
173 
174  // check if older similar polygon exists already and replace it with the new one
175  for (auto & polygon_stamped : data_) {
176  if (msg->polygon.id == polygon_stamped.polygon.id) {
177  polygon_stamped = *msg;
178  return;
179  }
180  }
181  data_.push_back(*msg);
182 }
183 
184 } // namespace nav2_collision_monitor
A QoS profile for best-effort sensor data with a history of 10 messages.
void getParameters(std::string &source_topic)
Getting sensor-specific ROS-parameters.
void dataCallback(geometry_msgs::msg::PolygonInstanceStamped::ConstSharedPtr msg)
PolygonSource data callback.
PolygonSource(const nav2::LifecycleNode::WeakPtr &node, const std::string &source_name, const nav2::TransformBuffer::SharedPtr tf_buffer, const std::string &base_frame_id, const std::string &global_frame_id, const tf2::Duration &transform_tolerance, const rclcpp::Duration &source_timeout, const bool base_shift_correction)
PolygonSource constructor.
nav2::Subscription< geometry_msgs::msg::PolygonInstanceStamped >::SharedPtr data_sub_
PolygonSource data subscriber.
void convertPolygonStampedToPoints(const geometry_msgs::msg::PolygonStamped &polygon, std::vector< Point > &data) const
Converts a PolygonInstanceStamped to a std::vector<Point>
~PolygonSource()
PolygonSource destructor.
bool configure()
Data source configuration routine. Obtains ROS-parameters and creates subscriber.
bool getSourceData(const rclcpp::Time &curr_time, std::vector< Point > &data) override
Adds latest data from polygon source to the data array.
double sampling_distance_
distance between sampled points on polygon edges
std::vector< geometry_msgs::msg::PolygonInstanceStamped > data_
Latest data obtained.
Basic data source class.
Definition: source.hpp:48
nav2::LifecycleNode::WeakPtr node_
Collision Monitor node.
Definition: source.hpp:209
bool getTransform(const rclcpp::Time &curr_time, const std_msgs::msg::Header &data_header, tf2::Transform &tf_transform) const
Obtain the transform to get data from source frame and time where it was received to the base frame a...
Definition: source.cpp:328
void getCommonParameters(std::string &source_topic)
Supporting routine obtaining ROS-parameters common for all data sources.
Definition: source.cpp:160
bool configure()
Source configuration routine.
Definition: source.cpp:62
std::string source_name_
Name of data source.
Definition: source.hpp:219
bool base_shift_correction_
Whether to correct source data towards to base frame movement, considering the difference between cur...
Definition: source.hpp:236
Point with 2D collision-check coordinates and optional z from the source.
Definition: types.hpp:52