Skip to content

Commit 22fa412

Browse files
authored
Merge branch 'main' into chore/stop_ignore_perception_spell_check
2 parents 70e9136 + c1b8f46 commit 22fa412

File tree

27 files changed

+622
-4692
lines changed

27 files changed

+622
-4692
lines changed

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -121,7 +121,7 @@ class LaneDepartureChecker
121121
std::vector<std::pair<double, lanelet::Lanelet>> getLaneletsFromPath(
122122
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
123123

124-
std::optional<lanelet::BasicPolygon2d> getFusedLaneletPolygonForPath(
124+
std::optional<tier4_autoware_utils::Polygon2d> getFusedLaneletPolygonForPath(
125125
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
126126

127127
bool checkPathWillLeaveLane(

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

+25-19
Original file line numberDiff line numberDiff line change
@@ -321,33 +321,39 @@ std::vector<std::pair<double, lanelet::Lanelet>> LaneDepartureChecker::getLanele
321321
lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0);
322322
}
323323

324-
std::optional<lanelet::BasicPolygon2d> LaneDepartureChecker::getFusedLaneletPolygonForPath(
324+
std::optional<tier4_autoware_utils::Polygon2d> LaneDepartureChecker::getFusedLaneletPolygonForPath(
325325
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
326326
{
327327
const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path);
328+
auto to_polygon2d = [](const lanelet::BasicPolygon2d & poly) -> tier4_autoware_utils::Polygon2d {
329+
tier4_autoware_utils::Polygon2d p;
330+
auto & outer = p.outer();
331+
332+
for (const auto & p : poly) {
333+
tier4_autoware_utils::Point2d p2d(p.x(), p.y());
334+
outer.push_back(p2d);
335+
}
336+
boost::geometry::correct(p);
337+
return p;
338+
};
339+
328340
// Fuse lanelets into a single BasicPolygon2d
329-
auto fused_lanelets = [&lanelets_distance_pair]() -> std::optional<lanelet::BasicPolygon2d> {
341+
auto fused_lanelets = [&]() -> std::optional<tier4_autoware_utils::Polygon2d> {
330342
if (lanelets_distance_pair.empty()) return std::nullopt;
331-
if (lanelets_distance_pair.size() == 1)
332-
return lanelets_distance_pair.at(0).second.polygon2d().basicPolygon();
343+
tier4_autoware_utils::MultiPolygon2d lanelet_unions;
344+
tier4_autoware_utils::MultiPolygon2d result;
333345

334-
lanelet::BasicPolygon2d merged_polygon =
335-
lanelets_distance_pair.at(0).second.polygon2d().basicPolygon();
336-
for (size_t i = 1; i < lanelets_distance_pair.size(); ++i) {
346+
for (size_t i = 0; i < lanelets_distance_pair.size(); ++i) {
337347
const auto & route_lanelet = lanelets_distance_pair.at(i).second;
338-
const auto & poly = route_lanelet.polygon2d().basicPolygon();
339-
340-
std::vector<lanelet::BasicPolygon2d> lanelet_union_temp;
341-
boost::geometry::union_(poly, merged_polygon, lanelet_union_temp);
342-
343-
// Update merged_polygon by accumulating all merged results
344-
merged_polygon.clear();
345-
for (const auto & temp_poly : lanelet_union_temp) {
346-
merged_polygon.insert(merged_polygon.end(), temp_poly.begin(), temp_poly.end());
347-
}
348+
const auto & p = route_lanelet.polygon2d().basicPolygon();
349+
tier4_autoware_utils::Polygon2d poly = to_polygon2d(p);
350+
boost::geometry::union_(lanelet_unions, poly, result);
351+
lanelet_unions = result;
352+
result.clear();
348353
}
349-
if (merged_polygon.empty()) return std::nullopt;
350-
return merged_polygon;
354+
355+
if (lanelet_unions.empty()) return std::nullopt;
356+
return lanelet_unions.front();
351357
}();
352358

353359
return fused_lanelets;

control/mpc_lateral_controller/src/mpc.cpp

+5-1
Original file line numberDiff line numberDiff line change
@@ -355,12 +355,15 @@ std::pair<bool, VectorXd> MPC::updateStateForDelayCompensation(
355355
MatrixXd Wd(DIM_X, 1);
356356
MatrixXd Cd(DIM_Y, DIM_X);
357357

358+
const double sign_vx = m_is_forward_shift ? 1 : -1;
359+
358360
MatrixXd x_curr = x0_orig;
359361
double mpc_curr_time = start_time;
360362
for (size_t i = 0; i < m_input_buffer.size(); ++i) {
361363
double k, v = 0.0;
362364
try {
363-
k = interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time);
365+
// NOTE: When driving backward, the curvature's sign should be reversed.
366+
k = interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time) * sign_vx;
364367
v = interpolation::lerp(traj.relative_time, traj.vx, mpc_curr_time);
365368
} catch (const std::exception & e) {
366369
RCLCPP_ERROR(m_logger, "mpc resample failed at delay compensation, stop mpc: %s", e.what());
@@ -446,6 +449,7 @@ MPCMatrix MPC::generateMPCMatrix(
446449
const double ref_vx = reference_trajectory.vx.at(i);
447450
const double ref_vx_squared = ref_vx * ref_vx;
448451

452+
// NOTE: When driving backward, the curvature's sign should be reversed.
449453
const double ref_k = reference_trajectory.k.at(i) * sign_vx;
450454
const double ref_smooth_k = reference_trajectory.smooth_k.at(i) * sign_vx;
451455

evaluator/perception_online_evaluator/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
<depend>diagnostic_msgs</depend>
2222
<depend>eigen</depend>
2323
<depend>geometry_msgs</depend>
24+
<depend>lanelet2_extension</depend>
2425
<depend>libgoogle-glog-dev</depend>
2526
<depend>motion_utils</depend>
2627
<depend>nav_msgs</depend>

localization/ndt_scan_matcher/src/map_update_module.cpp

+10-1
Original file line numberDiff line numberDiff line change
@@ -78,7 +78,16 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
7878

7979
ndt_ptr_->setParams(param);
8080

81-
update_ndt(position, *ndt_ptr_);
81+
const bool updated = update_ndt(position, *ndt_ptr_);
82+
if (!updated) {
83+
RCLCPP_ERROR_STREAM_THROTTLE(
84+
logger_, *clock_, 1000,
85+
"update_ndt failed. If this happens with initial position estimation, make sure that"
86+
"(1) the initial position matches the pcd map and (2) the map_loader is working properly.");
87+
last_update_position_ = position;
88+
ndt_ptr_mutex_->unlock();
89+
return;
90+
}
8291
ndt_ptr_->setInputSource(input_source);
8392
ndt_ptr_mutex_->unlock();
8493
need_rebuild_ = false;

perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp

+57-29
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
#define GROUND_SEGMENTATION__SCAN_GROUND_FILTER_NODELET_HPP_
1717

1818
#include "pointcloud_preprocessor/filter.hpp"
19+
#include "pointcloud_preprocessor/transform_info.hpp"
1920

2021
#include <vehicle_info_util/vehicle_info.hpp>
2122

@@ -50,7 +51,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
5051
// classified point label
5152
// (0: not classified, 1: ground, 2: not ground, 3: follow previous point,
5253
// 4: unkown(currently not used), 5: virtual ground)
53-
enum class PointLabel {
54+
enum class PointLabel : uint16_t {
5455
INIT = 0,
5556
GROUND,
5657
NON_GROUND,
@@ -59,19 +60,15 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
5960
VIRTUAL_GROUND,
6061
OUT_OF_RANGE
6162
};
62-
struct PointRef
63+
64+
struct PointData
6365
{
64-
float grid_size; // radius of grid
65-
uint16_t grid_id; // id of grid in vertical
66-
float radius; // cylindrical coords on XY Plane
67-
float theta; // angle deg on XY plane
68-
size_t radial_div; // index of the radial division to which this point belongs to
66+
float radius; // cylindrical coords on XY Plane
6967
PointLabel point_state{PointLabel::INIT};
70-
68+
uint16_t grid_id; // id of grid in vertical
7169
size_t orig_index; // index of this point in the source pointcloud
72-
pcl::PointXYZ * orig_point;
7370
};
74-
using PointCloudRefVector = std::vector<PointRef>;
71+
using PointCloudVector = std::vector<PointData>;
7572

7673
struct GridCenter
7774
{
@@ -144,33 +141,57 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
144141

145142
pcl::PointIndices getIndices() { return pcl_indices; }
146143
std::vector<float> getHeightList() { return height_list; }
144+
145+
pcl::PointIndices & getIndicesRef() { return pcl_indices; }
146+
std::vector<float> & getHeightListRef() { return height_list; }
147147
};
148148

149149
void filter(
150150
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override;
151151

152+
// TODO(taisa1): Temporary Implementation: Remove this interface when all the filter nodes
153+
// conform to new API
154+
virtual void faster_filter(
155+
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output,
156+
const pointcloud_preprocessor::TransformInfo & transform_info);
157+
152158
tf2_ros::Buffer tf_buffer_{get_clock()};
153159
tf2_ros::TransformListener tf_listener_{tf_buffer_};
154160

161+
int x_offset_;
162+
int y_offset_;
163+
int z_offset_;
164+
int intensity_offset_;
165+
bool offset_initialized_;
166+
167+
void set_field_offsets(const PointCloud2ConstPtr & input);
168+
169+
void get_point_from_global_offset(
170+
const PointCloud2ConstPtr & input, pcl::PointXYZ & point, size_t global_offset);
171+
155172
const uint16_t gnd_grid_continual_thresh_ = 3;
156173
bool elevation_grid_mode_;
157174
float non_ground_height_threshold_;
158175
float grid_size_rad_;
176+
float tan_grid_size_rad_;
159177
float grid_size_m_;
160178
float low_priority_region_x_;
161179
uint16_t gnd_grid_buffer_size_;
162180
float grid_mode_switch_grid_id_;
163181
float grid_mode_switch_angle_rad_;
164182
float virtual_lidar_z_;
165183
float detection_range_z_max_;
166-
float center_pcl_shift_; // virtual center of pcl to center mass
167-
float grid_mode_switch_radius_; // non linear grid size switching distance
168-
double global_slope_max_angle_rad_; // radians
169-
double local_slope_max_angle_rad_; // radians
184+
float center_pcl_shift_; // virtual center of pcl to center mass
185+
float grid_mode_switch_radius_; // non linear grid size switching distance
186+
double global_slope_max_angle_rad_; // radians
187+
double local_slope_max_angle_rad_; // radians
188+
double global_slope_max_ratio_;
189+
double local_slope_max_ratio_;
170190
double radial_divider_angle_rad_; // distance in rads between dividers
171191
double split_points_distance_tolerance_; // distance in meters between concentric divisions
172-
double // minimum height threshold regardless the slope,
173-
split_height_distance_; // useful for close points
192+
double split_points_distance_tolerance_square_;
193+
double // minimum height threshold regardless the slope,
194+
split_height_distance_; // useful for close points
174195
bool use_virtual_ground_point_;
175196
bool use_recheck_ground_cluster_; // to enable recheck ground cluster
176197
size_t radial_dividers_num_;
@@ -186,23 +207,25 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
186207
*/
187208

188209
/*!
189-
* Convert pcl::PointCloud to sorted PointCloudRefVector
210+
* Convert sensor_msgs::msg::PointCloud2 to sorted PointCloudVector
190211
* @param[in] in_cloud Input Point Cloud to be organized in radial segments
191212
* @param[out] out_radial_ordered_points_manager Vector of Points Clouds,
192213
* each element will contain the points ordered
193214
*/
194215
void convertPointcloud(
195-
const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
196-
std::vector<PointCloudRefVector> & out_radial_ordered_points_manager);
216+
const PointCloud2ConstPtr & in_cloud,
217+
std::vector<PointCloudVector> & out_radial_ordered_points_manager);
197218
void convertPointcloudGridScan(
198-
const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud,
199-
std::vector<PointCloudRefVector> & out_radial_ordered_points_manager);
219+
const PointCloud2ConstPtr & in_cloud,
220+
std::vector<PointCloudVector> & out_radial_ordered_points_manager);
200221
/*!
201222
* Output ground center of front wheels as the virtual ground point
202223
* @param[out] point Virtual ground origin point
203224
*/
204225
void calcVirtualGroundOrigin(pcl::PointXYZ & point);
205226

227+
float calcGridSize(const PointData & p);
228+
206229
/*!
207230
* Classifies Points in the PointCloud as Ground and Not Ground
208231
* @param in_radial_ordered_clouds Vector of an Ordered PointsCloud
@@ -214,14 +237,19 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
214237
void initializeFirstGndGrids(
215238
const float h, const float r, const uint16_t id, std::vector<GridCenter> & gnd_grids);
216239

217-
void checkContinuousGndGrid(PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
218-
void checkDiscontinuousGndGrid(PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
219-
void checkBreakGndGrid(PointRef & p, const std::vector<GridCenter> & gnd_grids_list);
240+
void checkContinuousGndGrid(
241+
PointData & p, pcl::PointXYZ & p_orig_point, const std::vector<GridCenter> & gnd_grids_list);
242+
void checkDiscontinuousGndGrid(
243+
PointData & p, pcl::PointXYZ & p_orig_point, const std::vector<GridCenter> & gnd_grids_list);
244+
void checkBreakGndGrid(
245+
PointData & p, pcl::PointXYZ & p_orig_point, const std::vector<GridCenter> & gnd_grids_list);
220246
void classifyPointCloud(
221-
std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
247+
const PointCloud2ConstPtr & in_cloud_ptr,
248+
std::vector<PointCloudVector> & in_radial_ordered_clouds,
222249
pcl::PointIndices & out_no_ground_indices);
223250
void classifyPointCloudGridScan(
224-
std::vector<PointCloudRefVector> & in_radial_ordered_clouds,
251+
const PointCloud2ConstPtr & in_cloud_ptr,
252+
std::vector<PointCloudVector> & in_radial_ordered_clouds,
225253
pcl::PointIndices & out_no_ground_indices);
226254
/*!
227255
* Re-classifies point of ground cluster based on their height
@@ -237,11 +265,11 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter
237265
* and the other removed as indicated in the indices
238266
* @param in_cloud_ptr Input PointCloud to which the extraction will be performed
239267
* @param in_indices Indices of the points to be both removed and kept
240-
* @param out_object_cloud_ptr Resulting PointCloud with the indices kept
268+
* @param out_object_cloud Resulting PointCloud with the indices kept
241269
*/
242270
void extractObjectPoints(
243-
const pcl::PointCloud<pcl::PointXYZ>::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices,
244-
pcl::PointCloud<pcl::PointXYZ>::Ptr out_object_cloud_ptr);
271+
const PointCloud2ConstPtr & in_cloud_ptr, const pcl::PointIndices & in_indices,
272+
PointCloud2 & out_object_cloud);
245273

246274
/** \brief Parameter service callback result : needed to be hold */
247275
OnSetParametersCallbackHandle::SharedPtr set_param_res_;

0 commit comments

Comments
 (0)