|
16 | 16 | #define AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ |
17 | 17 |
|
18 | 18 | #include <diagnostic_updater/diagnostic_updater.hpp> |
| 19 | +#include <motion_utils/trajectory/trajectory.hpp> |
19 | 20 | #include <pcl_ros/transforms.hpp> |
20 | 21 | #include <rclcpp/rclcpp.hpp> |
21 | 22 | #include <tier4_autoware_utils/geometry/geometry.hpp> |
|
40 | 41 | #include <tf2_ros/buffer.h> |
41 | 42 | #include <tf2_ros/transform_listener.h> |
42 | 43 |
|
| 44 | +#include <deque> |
| 45 | +#include <limits> |
43 | 46 | #include <memory> |
44 | | -#include <mutex> |
45 | 47 | #include <optional> |
46 | 48 | #include <string> |
| 49 | +#include <utility> |
47 | 50 | #include <vector> |
48 | | - |
49 | 51 | namespace autoware::motion::control::autonomous_emergency_braking |
50 | 52 | { |
51 | 53 |
|
@@ -79,30 +81,142 @@ class CollisionDataKeeper |
79 | 81 | public: |
80 | 82 | explicit CollisionDataKeeper(rclcpp::Clock::SharedPtr clock) { clock_ = clock; } |
81 | 83 |
|
82 | | - void setTimeout(const double timeout_sec) { timeout_sec_ = timeout_sec; } |
| 84 | + void setTimeout(const double collision_keep_time, const double previous_obstacle_keep_time) |
| 85 | + { |
| 86 | + collision_keep_time_ = collision_keep_time; |
| 87 | + previous_obstacle_keep_time_ = previous_obstacle_keep_time; |
| 88 | + } |
83 | 89 |
|
84 | | - bool checkExpired() |
| 90 | + bool checkObjectDataExpired(std::optional<ObjectData> & data, const double timeout) |
85 | 91 | { |
86 | | - if (data_ && (clock_->now() - data_->stamp).seconds() > timeout_sec_) { |
87 | | - data_.reset(); |
| 92 | + if (!data.has_value()) return true; |
| 93 | + const auto now = clock_->now(); |
| 94 | + const auto & prev_obj = data.value(); |
| 95 | + const auto & data_time_stamp = prev_obj.stamp; |
| 96 | + if ((now - data_time_stamp).nanoseconds() * 1e-9 > timeout) { |
| 97 | + data = std::nullopt; |
| 98 | + return true; |
88 | 99 | } |
89 | | - return (data_ == nullptr); |
| 100 | + return false; |
| 101 | + } |
| 102 | + |
| 103 | + bool checkCollisionExpired() |
| 104 | + { |
| 105 | + return this->checkObjectDataExpired(closest_object_, collision_keep_time_); |
90 | 106 | } |
91 | 107 |
|
92 | | - void update(const ObjectData & data) { data_.reset(new ObjectData(data)); } |
| 108 | + bool checkPreviousObjectDataExpired() |
| 109 | + { |
| 110 | + return this->checkObjectDataExpired(prev_closest_object_, previous_obstacle_keep_time_); |
| 111 | + } |
93 | 112 |
|
94 | | - ObjectData get() |
| 113 | + ObjectData get() const |
95 | 114 | { |
96 | | - if (data_) { |
97 | | - return *data_; |
98 | | - } else { |
99 | | - return ObjectData(); |
| 115 | + return (closest_object_.has_value()) ? closest_object_.value() : ObjectData(); |
| 116 | + } |
| 117 | + |
| 118 | + ObjectData getPreviousObjectData() const |
| 119 | + { |
| 120 | + return (prev_closest_object_.has_value()) ? prev_closest_object_.value() : ObjectData(); |
| 121 | + } |
| 122 | + |
| 123 | + void setCollisionData(const ObjectData & data) |
| 124 | + { |
| 125 | + closest_object_ = std::make_optional<ObjectData>(data); |
| 126 | + } |
| 127 | + |
| 128 | + void setPreviousObjectData(const ObjectData & data) |
| 129 | + { |
| 130 | + prev_closest_object_ = std::make_optional<ObjectData>(data); |
| 131 | + } |
| 132 | + |
| 133 | + void resetVelocityHistory() { obstacle_velocity_history_.clear(); } |
| 134 | + |
| 135 | + void updateVelocityHistory( |
| 136 | + const double current_object_velocity, const rclcpp::Time & current_object_velocity_time_stamp) |
| 137 | + { |
| 138 | + // remove old msg from deque |
| 139 | + const auto now = clock_->now(); |
| 140 | + std::remove_if( |
| 141 | + obstacle_velocity_history_.begin(), obstacle_velocity_history_.end(), |
| 142 | + [&](const auto & velocity_time_pair) { |
| 143 | + const auto & vel_time = velocity_time_pair.second; |
| 144 | + return ((now - vel_time).nanoseconds() * 1e-9 > previous_obstacle_keep_time_); |
| 145 | + }); |
| 146 | + |
| 147 | + obstacle_velocity_history_.emplace_back( |
| 148 | + std::make_pair(current_object_velocity, current_object_velocity_time_stamp)); |
| 149 | + } |
| 150 | + |
| 151 | + std::optional<double> getMedianObstacleVelocity() const |
| 152 | + { |
| 153 | + if (obstacle_velocity_history_.empty()) return std::nullopt; |
| 154 | + std::vector<double> raw_velocities; |
| 155 | + for (const auto & vel_time_pair : obstacle_velocity_history_) { |
| 156 | + raw_velocities.emplace_back(vel_time_pair.first); |
| 157 | + } |
| 158 | + |
| 159 | + const size_t med1 = (raw_velocities.size() % 2 == 0) ? (raw_velocities.size()) / 2 - 1 |
| 160 | + : (raw_velocities.size()) / 2.0; |
| 161 | + const size_t med2 = (raw_velocities.size()) / 2.0; |
| 162 | + std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med1, raw_velocities.end()); |
| 163 | + const double vel1 = raw_velocities.at(med1); |
| 164 | + std::nth_element(raw_velocities.begin(), raw_velocities.begin() + med2, raw_velocities.end()); |
| 165 | + const double vel2 = raw_velocities.at(med2); |
| 166 | + return (vel1 + vel2) / 2.0; |
| 167 | + } |
| 168 | + |
| 169 | + std::optional<double> calcObjectSpeedFromHistory( |
| 170 | + const ObjectData & closest_object, const Path & path, const double current_ego_speed) |
| 171 | + { |
| 172 | + if (this->checkPreviousObjectDataExpired()) { |
| 173 | + this->setPreviousObjectData(closest_object); |
| 174 | + this->resetVelocityHistory(); |
| 175 | + return std::nullopt; |
| 176 | + } |
| 177 | + |
| 178 | + const auto estimated_velocity_opt = std::invoke([&]() -> std::optional<double> { |
| 179 | + const auto & prev_object = this->getPreviousObjectData(); |
| 180 | + const double p_dt = |
| 181 | + (closest_object.stamp.nanoseconds() - prev_object.stamp.nanoseconds()) * 1e-9; |
| 182 | + if (p_dt < std::numeric_limits<double>::epsilon()) return std::nullopt; |
| 183 | + const auto & nearest_collision_point = closest_object.position; |
| 184 | + const auto & prev_collision_point = prev_object.position; |
| 185 | + |
| 186 | + const double p_dx = nearest_collision_point.x - prev_collision_point.x; |
| 187 | + const double p_dy = nearest_collision_point.y - prev_collision_point.y; |
| 188 | + const double p_dist = std::hypot(p_dx, p_dy); |
| 189 | + const double p_yaw = std::atan2(p_dy, p_dx); |
| 190 | + const double p_vel = p_dist / p_dt; |
| 191 | + |
| 192 | + const auto nearest_idx = motion_utils::findNearestIndex(path, nearest_collision_point); |
| 193 | + const auto & nearest_path_pose = path.at(nearest_idx); |
| 194 | + const auto & traj_yaw = tf2::getYaw(nearest_path_pose.orientation); |
| 195 | + const auto estimated_velocity = p_vel * std::cos(p_yaw - traj_yaw) + current_ego_speed; |
| 196 | + |
| 197 | + // Current RSS distance calculation does not account for negative velocities |
| 198 | + return (estimated_velocity > 0.0) ? estimated_velocity : 0.0; |
| 199 | + }); |
| 200 | + |
| 201 | + if (!estimated_velocity_opt.has_value()) { |
| 202 | + this->setPreviousObjectData(closest_object); |
| 203 | + this->resetVelocityHistory(); |
| 204 | + return std::nullopt; |
100 | 205 | } |
| 206 | + |
| 207 | + const auto & estimated_velocity = estimated_velocity_opt.value(); |
| 208 | + this->setPreviousObjectData(closest_object); |
| 209 | + this->updateVelocityHistory(estimated_velocity, closest_object.stamp); |
| 210 | + return this->getMedianObstacleVelocity(); |
101 | 211 | } |
102 | 212 |
|
103 | 213 | private: |
104 | | - std::unique_ptr<ObjectData> data_; |
105 | | - double timeout_sec_{0.0}; |
| 214 | + std::optional<ObjectData> prev_closest_object_{std::nullopt}; |
| 215 | + std::optional<ObjectData> closest_object_{std::nullopt}; |
| 216 | + double collision_keep_time_{0.0}; |
| 217 | + double previous_obstacle_keep_time_{0.0}; |
| 218 | + |
| 219 | + std::deque<std::pair<double, rclcpp::Time>> obstacle_velocity_history_; |
106 | 220 | rclcpp::Clock::SharedPtr clock_; |
107 | 221 | }; |
108 | 222 |
|
@@ -152,14 +266,22 @@ class AEB : public rclcpp::Node |
152 | 266 | void cropPointCloudWithEgoFootprintPath( |
153 | 267 | const std::vector<Polygon2d> & ego_polys, pcl::PointCloud<pcl::PointXYZ>:: Ptr filtered_objects); |
154 | 268 |
|
| 269 | + void createObjectDataUsingPointCloudClusters( |
| 270 | + const Path & ego_path, const std::vector<Polygon2d> & ego_polys, const rclcpp::Time & stamp, |
| 271 | + std::vector<ObjectData> & objects); |
| 272 | + void cropPointCloudWithEgoFootprintPath( const std::vector<Polygon2d> & ego_polys); |
| 273 | + |
155 | 274 | void addMarker( |
156 | 275 | const rclcpp::Time & current_time, const Path & path, const std::vector<Polygon2d> & polygons, |
157 | | - const std::vector<ObjectData> & objects, const double color_r, const double color_g, |
158 | | - const double color_b, const double color_a, const std::string & ns, |
159 | | - MarkerArray & debug_markers); |
| 276 | + const std::vector<ObjectData> & objects, const std::optional<ObjectData> & closest_object, |
| 277 | + const double color_r, const double color_g, const double color_b, const double color_a, |
| 278 | + const std::string & ns, MarkerArray & debug_markers); |
160 | 279 |
|
161 | 280 | void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers); |
162 | 281 |
|
| 282 | + std::optional<double> calcObjectSpeedFromHistory( |
| 283 | + const ObjectData & closest_object, const Path & path, const double current_ego_speed); |
| 284 | + |
163 | 285 | PointCloud2::SharedPtr obstacle_ros_pointcloud_ptr_{nullptr}; |
164 | 286 | VelocityReport::ConstSharedPtr current_velocity_ptr_{nullptr}; |
165 | 287 | Vector3::SharedPtr angular_velocity_ptr_{nullptr}; |
|
0 commit comments