diff --git a/common/autoware_trajectory/package.xml b/common/autoware_trajectory/package.xml
index 8ce9cedd40d8f..bf935724684bc 100644
--- a/common/autoware_trajectory/package.xml
+++ b/common/autoware_trajectory/package.xml
@@ -20,7 +20,6 @@
lanelet2_core
rclcpp
tf2
- tier4_planning_msgs
ament_cmake_ros
ament_lint_auto
diff --git a/perception/autoware_map_based_prediction/README.md b/perception/autoware_map_based_prediction/README.md
index bd063050b9241..4541f3d7c65c2 100644
--- a/perception/autoware_map_based_prediction/README.md
+++ b/perception/autoware_map_based_prediction/README.md
@@ -230,7 +230,6 @@ If the target object is inside the road or crosswalk, this module outputs one or
| Parameter | Unit | Type | Description |
| ---------------------------------------------------------------- | ----- | ------ | ------------------------------------------------------------------------------------------------------------------------------------- |
-| `enable_delay_compensation` | [-] | bool | flag to enable the time delay compensation for the position of the object |
| `prediction_time_horizon` | [s] | double | predict time duration for predicted path |
| `lateral_control_time_horizon` | [s] | double | time duration for predicted path will reach the reference path (mostly center of the lane) |
| `prediction_sampling_delta_time` | [s] | double | sampling time for points in predicted path |
diff --git a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
index b047cd28114ba..4ed77af694b80 100644
--- a/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
+++ b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -117,9 +117,6 @@ class MapBasedPredictionNode : public rclcpp::Node
////// Parameters
- // Object Parameters
- bool enable_delay_compensation_;
-
//// Vehicle Parameters
// Lanelet Parameters
double dist_threshold_for_searching_lanelet_;
@@ -174,10 +171,6 @@ class MapBasedPredictionNode : public rclcpp::Node
// Object process
PredictedObject convertToPredictedObject(const TrackedObject & tracked_object);
void updateObjectData(TrackedObject & object);
- geometry_msgs::msg::Pose compensateTimeDelay(
- const geometry_msgs::msg::Pose & delayed_pose, const geometry_msgs::msg::Twist & twist,
- const double dt) const;
-
//// Vehicle process
// Lanelet process
LaneletsData getCurrentLanelets(const TrackedObject & object);
diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
index b65020d6b4708..f32a693034431 100644
--- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
@@ -364,7 +364,6 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
google::InitGoogleLogging("map_based_prediction_node");
google::InstallFailureSignalHandler();
}
- enable_delay_compensation_ = declare_parameter("enable_delay_compensation");
prediction_time_horizon_.vehicle = declare_parameter("prediction_time_horizon.vehicle");
prediction_time_horizon_.pedestrian =
declare_parameter("prediction_time_horizon.pedestrian");
@@ -1246,42 +1245,6 @@ Maneuver MapBasedPredictionNode::predictObjectManeuverByLatDiffDistance(
return Maneuver::LANE_FOLLOW;
}
-geometry_msgs::msg::Pose MapBasedPredictionNode::compensateTimeDelay(
- const geometry_msgs::msg::Pose & delayed_pose, const geometry_msgs::msg::Twist & twist,
- const double dt) const
-{
- if (!enable_delay_compensation_) {
- return delayed_pose;
- }
-
- /* == Nonlinear model ==
- *
- * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt - vy_k * sin(yaw_k) * dt
- * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt + vy_k * cos(yaw_k) * dt
- * yaw_{k+1} = yaw_k + (wz_k) * dt
- *
- */
-
- const double vx = twist.linear.x;
- const double vy = twist.linear.y;
- const double wz = twist.angular.z;
- const double prev_yaw = tf2::getYaw(delayed_pose.orientation);
- const double prev_x = delayed_pose.position.x;
- const double prev_y = delayed_pose.position.y;
- const double prev_z = delayed_pose.position.z;
-
- const double curr_x = prev_x + vx * std::cos(prev_yaw) * dt - vy * std::sin(prev_yaw) * dt;
- const double curr_y = prev_y + vx * std::sin(prev_yaw) * dt + vy * std::cos(prev_yaw) * dt;
- const double curr_z = prev_z;
- const double curr_yaw = prev_yaw + wz * dt;
-
- geometry_msgs::msg::Pose current_pose;
- current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, curr_z);
- current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw);
-
- return current_pose;
-}
-
double MapBasedPredictionNode::calcRightLateralOffset(
const lanelet::ConstLineString2d & boundary_line, const geometry_msgs::msg::Pose & search_pose)
{
diff --git a/perception/autoware_traffic_light_arbiter/README.md b/perception/autoware_traffic_light_arbiter/README.md
index f27afab62818e..c35a2f04ec9c8 100644
--- a/perception/autoware_traffic_light_arbiter/README.md
+++ b/perception/autoware_traffic_light_arbiter/README.md
@@ -39,12 +39,4 @@ The table below outlines how the matching process determines the output based on
## Parameters
-### Core Parameters
-
-| Name | Type | Default Value | Description |
-| --------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ |
-| `external_delay_tolerance` | double | 5.0 | The duration in seconds an external message is considered valid for merging in comparison with current time |
-| `external_time_tolerance` | double | 5.0 | The duration in seconds an external message is considered valid for merging in comparison with a timestamp of perception message |
-| `perception_time_tolerance` | double | 1.0 | The duration in seconds a perception message is considered valid for merging in comparison with a timestamp of external message |
-| `external_priority` | bool | false | Whether or not externals signals take precedence over perception-based ones. If false, the merging uses confidence as a criteria |
-| `enable_signal_matching` | bool | false | Decide whether to validate the match between perception signals and external signals. If set to true, verify that the colors match and only publish them if they are identical |
+{{ json_to_markdown("perception/autoware_traffic_light_arbiter/schema/traffic_light_arbiter.schema.json") }}
diff --git a/perception/autoware_traffic_light_arbiter/schema/traffic_light_arbiter.schema.json b/perception/autoware_traffic_light_arbiter/schema/traffic_light_arbiter.schema.json
new file mode 100644
index 0000000000000..284f474509f25
--- /dev/null
+++ b/perception/autoware_traffic_light_arbiter/schema/traffic_light_arbiter.schema.json
@@ -0,0 +1,59 @@
+{
+ "$schema": "http://json-schema.org/draft-07/schema#",
+ "title": "autoware_traffic_light_arbiter parameters",
+ "type": "object",
+ "definitions": {
+ "traffic_light_arbiter": {
+ "type": "object",
+ "properties": {
+ "external_delay_tolerance": {
+ "type": "number",
+ "description": "The duration in seconds an external message is considered valid for merging in comparison with current time.",
+ "default": 5.0
+ },
+ "external_time_tolerance": {
+ "type": "number",
+ "description": "The duration in seconds an external message is considered valid for merging.",
+ "default": 5.0
+ },
+ "perception_time_tolerance": {
+ "type": "number",
+ "description": "The duration in seconds a perception message is considered valid for merging.",
+ "default": 1.0
+ },
+ "external_priority": {
+ "type": "boolean",
+ "description": "Whether or not external signals take precedence over perception-based ones. If false, the merging uses confidence as a criteria.",
+ "default": false
+ },
+ "enable_signal_matching": {
+ "type": "boolean",
+ "description": "Decide whether to validate the match between perception signals and external signals. If set to true, verify that the colors match and only publish them if they are identical.",
+ "default": false
+ }
+ },
+ "required": [
+ "external_delay_tolerance",
+ "external_time_tolerance",
+ "perception_time_tolerance",
+ "external_priority",
+ "enable_signal_matching"
+ ],
+ "additionalProperties": false
+ }
+ },
+ "properties": {
+ "/**": {
+ "type": "object",
+ "properties": {
+ "ros__parameters": {
+ "$ref": "#/definitions/traffic_light_arbiter"
+ }
+ },
+ "required": ["ros__parameters"],
+ "additionalProperties": false
+ }
+ },
+ "required": ["/**"],
+ "additionalProperties": false
+}
diff --git a/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp b/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp
index 8ce002780813f..5591d6cd036fe 100644
--- a/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp
+++ b/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp
@@ -71,11 +71,11 @@ namespace autoware
TrafficLightArbiter::TrafficLightArbiter(const rclcpp::NodeOptions & options)
: Node("traffic_light_arbiter", options)
{
- external_delay_tolerance_ = this->declare_parameter("external_delay_tolerance", 5.0);
- external_time_tolerance_ = this->declare_parameter("external_time_tolerance", 5.0);
- perception_time_tolerance_ = this->declare_parameter("perception_time_tolerance", 1.0);
- external_priority_ = this->declare_parameter("external_priority", false);
- enable_signal_matching_ = this->declare_parameter("enable_signal_matching", false);
+ external_delay_tolerance_ = this->declare_parameter("external_delay_tolerance");
+ external_time_tolerance_ = this->declare_parameter("external_time_tolerance");
+ perception_time_tolerance_ = this->declare_parameter("perception_time_tolerance");
+ external_priority_ = this->declare_parameter("external_priority");
+ enable_signal_matching_ = this->declare_parameter("enable_signal_matching");
if (enable_signal_matching_) {
signal_match_validator_ = std::make_unique();
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp
index db327e89a59a5..218d94316bfd8 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp
@@ -1666,22 +1666,14 @@ std::vector calcBound(
: postProcess(bound, path, planner_data, drivable_lanes, is_left, is_driving_forward);
};
- // Step2. if there is no drivable area defined by polygon, return original drivable bound.
- if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas) {
- return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process);
- }
-
- // Step3.if there are hatched road markings, expand drivable bound with the polygon.
+ // if there is no drivable area defined by polygon, return original drivable bound.
+ // if there are hatched road markings, expand drivable bound with the polygon.
+ // if there are intersection areas, expand drivable bound with the polygon.
if (enable_expanding_hatched_road_markings) {
bound_points = getBoundWithHatchedRoadMarkings(bound_points, route_handler);
}
- if (!enable_expanding_intersection_areas) {
- return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process);
- }
-
- // Step4. if there are intersection areas, expand drivable bound with the polygon.
- {
+ if (enable_expanding_intersection_areas) {
bound_points =
getBoundWithIntersectionAreas(bound_points, route_handler, drivable_lanes, is_left);
}