Skip to content

Commit fb5e2c2

Browse files
anhnv3991anhnv3991
authored
Merge branch 'main' into fix/add_error_handling_about_update_ndt
2 parents 75f24c4 + d57e97e commit fb5e2c2

File tree

31 files changed

+561
-193
lines changed

31 files changed

+561
-193
lines changed

‎evaluator/tier4_metrics_rviz_plugin/package.xml

+2
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,8 @@
77
<maintainer email="satoshi.ota@tier4.jp">Satoshi OTA</maintainer>
88
<maintainer email="kyoichi.sugahara@tier4.jp">Kyoichi Sugahara</maintainer>
99
<maintainer email="maxime.clement@tier4.jp">Maxime CLEMENT</maintainer>
10+
<maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</maintainer>
11+
<maintainer email="fumiya.watanabe@tier4.jp">Fumiya Watanabe</maintainer>
1012
<license>Apache License 2.0</license>
1113

1214
<buildtool_depend>ament_cmake_auto</buildtool_depend>

‎evaluator/tier4_metrics_rviz_plugin/src/metrics_visualize_panel.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -62,10 +62,10 @@ struct Metric
6262
{
6363
auto label = new QLabel;
6464
label->setAlignment(Qt::AlignCenter);
65-
label->setText("metric_name");
65+
label->setText(QString::fromStdString(status.name));
6666
labels.emplace("metric_name", label);
6767

68-
header.push_back(QString::fromStdString(status.name));
68+
header.push_back("metric_name");
6969
}
7070

7171
for (const auto & [key, value] : status.values) {

‎perception/radar_tracks_msgs_converter/README.md

+6
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,12 @@ Autoware objects label is defined in [ObjectClassification.idl](https://gitlab.c
5252

5353
### Parameters
5454

55+
#### Parameter Summary
56+
57+
{{ json_to_markdown("perception/radar_tracks_msgs_converter/schema/radar_tracks_msgs_converter.schema.json") }}
58+
59+
#### Parameter Description
60+
5561
- `update_rate_hz` (double) [hz]
5662
- Default parameter is 20.0
5763

‎perception/radar_tracks_msgs_converter/schema/radar_tracks_msgs_converter.schema.json

+57
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
{
2+
"$schema": "http://json-schema.org/draft-07/schema#",
3+
"title": "Parameters for Radar Tracks Msgs Converter node",
4+
"type": "object",
5+
"definitions": {
6+
"radar_tracks_msgs_converter": {
7+
"type": "object",
8+
"properties": {
9+
"update_rate_hz": {
10+
"type": "number",
11+
"description": "The update rate [hz] of the output topic",
12+
"default": "20.0",
13+
"minimum": 0.0
14+
},
15+
"new_frame_id": {
16+
"type": "string",
17+
"description": "The header frame_id of the output topic",
18+
"default": "base_link"
19+
},
20+
"use_twist_compensation": {
21+
"type": "boolean",
22+
"description": "Flag to enable the linear compensation of ego vehicle's twist",
23+
"default": "false"
24+
},
25+
"use_twist_yaw_compensation": {
26+
"type": "boolean",
27+
"description": "Flag to enable the compensation of yaw rotation of ego vehicle's twist",
28+
"default": "false"
29+
},
30+
"static_object_speed_threshold": {
31+
"type": "number",
32+
"description": "Threshold to treat detected objects as static objects",
33+
"default": "1.0"
34+
}
35+
},
36+
"required": [
37+
"update_rate_hz",
38+
"new_frame_id",
39+
"use_twist_compensation",
40+
"use_twist_yaw_compensation",
41+
"static_object_speed_threshold"
42+
]
43+
}
44+
},
45+
"properties": {
46+
"/**": {
47+
"type": "object",
48+
"properties": {
49+
"ros__parameters": {
50+
"$ref": "#/definitions/radar_tracks_msgs_converter"
51+
}
52+
},
53+
"required": ["ros__parameters"]
54+
}
55+
},
56+
"required": ["/**"]
57+
}

‎planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp

-4
Original file line numberDiff line numberDiff line change
@@ -141,10 +141,6 @@ bool hasEnoughLengthToLaneChangeAfterAbort(
141141
const Pose & curent_pose, const double abort_return_dist,
142142
const LaneChangeParameters & lane_change_parameters, const Direction direction);
143143

144-
lanelet::ConstLanelets getBackwardLanelets(
145-
const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
146-
const Pose & current_pose, const double backward_length);
147-
148144
double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0);
149145

150146
double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer);

‎planning/behavior_path_lane_change_module/src/scene.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -843,7 +843,7 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects(
843843

844844
// get backward lanes
845845
const auto backward_length = lane_change_parameters_->backward_lane_length;
846-
const auto target_backward_lanes = utils::lane_change::getBackwardLanelets(
846+
const auto target_backward_lanes = behavior_path_planner::utils::getBackwardLanelets(
847847
route_handler, target_lanes, current_pose, backward_length);
848848

849849
// filter objects to get target object index

‎planning/behavior_path_lane_change_module/src/utils/utils.cpp

-37
Original file line numberDiff line numberDiff line change
@@ -674,43 +674,6 @@ bool hasEnoughLengthToLaneChangeAfterAbort(
674674
return true;
675675
}
676676

677-
// TODO(Azu): In the future, get back lanelet within `to_back_dist` [m] from queried lane
678-
lanelet::ConstLanelets getBackwardLanelets(
679-
const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
680-
const Pose & current_pose, const double backward_length)
681-
{
682-
if (target_lanes.empty()) {
683-
return {};
684-
}
685-
686-
const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose);
687-
688-
if (arc_length.length >= backward_length) {
689-
return {};
690-
}
691-
692-
const auto & front_lane = target_lanes.front();
693-
const auto preceding_lanes = route_handler.getPrecedingLaneletSequence(
694-
front_lane, std::abs(backward_length - arc_length.length), {front_lane});
695-
696-
lanelet::ConstLanelets backward_lanes{};
697-
const auto num_of_lanes = std::invoke([&preceding_lanes]() {
698-
size_t sum{0};
699-
for (const auto & lanes : preceding_lanes) {
700-
sum += lanes.size();
701-
}
702-
return sum;
703-
});
704-
705-
backward_lanes.reserve(num_of_lanes);
706-
707-
for (const auto & lanes : preceding_lanes) {
708-
backward_lanes.insert(backward_lanes.end(), lanes.begin(), lanes.end());
709-
}
710-
711-
return backward_lanes;
712-
}
713-
714677
double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer)
715678
{
716679
return lateral_buffer + 0.5 * vehicle_width;

‎planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp

+4
Original file line numberDiff line numberDiff line change
@@ -299,6 +299,10 @@ lanelet::ConstLanelets extendPrevLane(
299299
lanelet::ConstLanelets extendLanes(
300300
const std::shared_ptr<RouteHandler> route_handler, const lanelet::ConstLanelets & lanes);
301301

302+
lanelet::ConstLanelets getBackwardLanelets(
303+
const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
304+
const Pose & current_pose, const double backward_length);
305+
302306
lanelet::ConstLanelets getExtendedCurrentLanes(
303307
const std::shared_ptr<const PlannerData> & planner_data);
304308

‎planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp

+4
Original file line numberDiff line numberDiff line change
@@ -1481,6 +1481,10 @@ std::vector<geometry_msgs::msg::Point> postProcess(
14811481
const auto & vehicle_length = planner_data->parameters.vehicle_length;
14821482
constexpr double overlap_threshold = 0.01;
14831483

1484+
if (original_bound.size() < 2) {
1485+
return original_bound;
1486+
}
1487+
14841488
const auto addPoints =
14851489
[](const lanelet::ConstLineString3d & points, std::vector<geometry_msgs::msg::Point> & bound) {
14861490
for (const auto & bound_p : points) {

‎planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -78,7 +78,7 @@ PredictedObjects filterObjects(
7878
const std::shared_ptr<ObjectsFilteringParams> & params)
7979
{
8080
// Guard
81-
if (objects->objects.empty()) {
81+
if (objects->objects.empty() || !params) {
8282
return PredictedObjects();
8383
}
8484

‎planning/behavior_path_planner_common/src/utils/utils.cpp

+37
Original file line numberDiff line numberDiff line change
@@ -1514,6 +1514,43 @@ lanelet::ConstLanelets getExtendedCurrentLanesFromPath(
15141514
return lanes;
15151515
}
15161516

1517+
// TODO(Azu): In the future, get back lanelet within `to_back_dist` [m] from queried lane
1518+
lanelet::ConstLanelets getBackwardLanelets(
1519+
const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
1520+
const Pose & current_pose, const double backward_length)
1521+
{
1522+
if (target_lanes.empty()) {
1523+
return {};
1524+
}
1525+
1526+
const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose);
1527+
1528+
if (arc_length.length >= backward_length) {
1529+
return {};
1530+
}
1531+
1532+
const auto & front_lane = target_lanes.front();
1533+
const auto preceding_lanes = route_handler.getPrecedingLaneletSequence(
1534+
front_lane, std::abs(backward_length - arc_length.length), {front_lane});
1535+
1536+
lanelet::ConstLanelets backward_lanes{};
1537+
const auto num_of_lanes = std::invoke([&preceding_lanes]() {
1538+
size_t sum{0};
1539+
for (const auto & lanes : preceding_lanes) {
1540+
sum += lanes.size();
1541+
}
1542+
return sum;
1543+
});
1544+
1545+
backward_lanes.reserve(num_of_lanes);
1546+
1547+
for (const auto & lanes : preceding_lanes) {
1548+
backward_lanes.insert(backward_lanes.end(), lanes.begin(), lanes.end());
1549+
}
1550+
1551+
return backward_lanes;
1552+
}
1553+
15171554
lanelet::ConstLanelets calcLaneAroundPose(
15181555
const std::shared_ptr<RouteHandler> route_handler, const Pose & pose, const double forward_length,
15191556
const double backward_length, const double dist_threshold, const double yaw_threshold)

0 commit comments

Comments
 (0)