Skip to content

Commit

Permalink
Merge branch 'main' into use-autoware-utils-for-interpolation
Browse files Browse the repository at this point in the history
  • Loading branch information
@mitsudome-r
mitsudome-r authored Feb 12, 2025
2 parents 7e56359 + bcec02e commit 6acf5ac
Show file tree
Hide file tree
Showing 8 changed files with 71 additions and 79 deletions.
1 change: 0 additions & 1 deletion common/autoware_trajectory/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,6 @@
<depend>lanelet2_core</depend>
<depend>rclcpp</depend>
<depend>tf2</depend>
<depend>tier4_planning_msgs</depend>

<test_depend>ament_cmake_ros</test_depend>
<test_depend>ament_lint_auto</test_depend>
Expand Down
1 change: 0 additions & 1 deletion perception/autoware_map_based_prediction/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 |
Expand Down
7 changes: 0 additions & 7 deletions .../autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand Down Expand Up @@ -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);
Expand Down
37 changes: 0 additions & 37 deletions perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -364,7 +364,6 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
google::InitGoogleLogging("map_based_prediction_node");
google::InstallFailureSignalHandler();
}
enable_delay_compensation_ = declare_parameter<bool>("enable_delay_compensation");
prediction_time_horizon_.vehicle = declare_parameter<double>("prediction_time_horizon.vehicle");
prediction_time_horizon_.pedestrian =
declare_parameter<double>("prediction_time_horizon.pedestrian");
Expand Down Expand Up @@ -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)
{
Expand Down
10 changes: 1 addition & 9 deletions perception/autoware_traffic_light_arbiter/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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") }}
59 changes: 59 additions & 0 deletions perception/autoware_traffic_light_arbiter/schema/traffic_light_arbiter.schema.json
View file
Original file line number Diff line number Diff line change
@@ -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
}
10 changes: 5 additions & 5 deletions perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,11 +71,11 @@ namespace autoware
TrafficLightArbiter::TrafficLightArbiter(const rclcpp::NodeOptions & options)
: Node("traffic_light_arbiter", options)
{
external_delay_tolerance_ = this->declare_parameter<double>("external_delay_tolerance", 5.0);
external_time_tolerance_ = this->declare_parameter<double>("external_time_tolerance", 5.0);
perception_time_tolerance_ = this->declare_parameter<double>("perception_time_tolerance", 1.0);
external_priority_ = this->declare_parameter<bool>("external_priority", false);
enable_signal_matching_ = this->declare_parameter<bool>("enable_signal_matching", false);
external_delay_tolerance_ = this->declare_parameter<double>("external_delay_tolerance");
external_time_tolerance_ = this->declare_parameter<double>("external_time_tolerance");
perception_time_tolerance_ = this->declare_parameter<double>("perception_time_tolerance");
external_priority_ = this->declare_parameter<bool>("external_priority");
enable_signal_matching_ = this->declare_parameter<bool>("enable_signal_matching");

if (enable_signal_matching_) {
signal_match_validator_ = std::make_unique<SignalMatchValidator>();
Expand Down
25 changes: 6 additions & 19 deletions ...e_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -662,20 +662,15 @@ std::optional<size_t> getOverlappedLaneletId(const std::vector<DrivableLanes> &
return {};
}

size_t overlapped_idx = lanes.size();
for (size_t i = 0; i < lanes.size() - 2; ++i) {
for (size_t j = i + 2; j < lanes.size(); ++j) {
if (overlaps(lanes.at(i), lanes.at(j))) {
overlapped_idx = std::min(overlapped_idx, j);
return j;
}
}
}

if (overlapped_idx == lanes.size()) {
return {};
}

return overlapped_idx;
return {};
}

std::vector<DrivableLanes> cutOverlappedLanes(
Expand Down Expand Up @@ -1671,22 +1666,14 @@ std::vector<geometry_msgs::msg::Point> 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);
}
Expand Down

0 comments on commit 6acf5ac

Please sign in to comment.