Skip to content

Commit

Permalink
Merge branch 'main' into feat/vehicle_cmd_gate_check_cmd_continuity
Browse files Browse the repository at this point in the history
  • Loading branch information
tkimura4 authored Aug 16, 2024
2 parents 0b70bfb + a612530 commit 9b6b2ac
Show file tree
Hide file tree
Showing 10 changed files with 10,292 additions and 18 deletions.
3 changes: 1 addition & 2 deletions evaluator/kinematic_evaluator/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -38,8 +38,7 @@ if(BUILD_TESTING)
)
endif()

ament_auto_package(
INSTALL_TO_SHARE
ament_auto_package(INSTALL_TO_SHARE
param
launch
)
4 changes: 4 additions & 0 deletions evaluator/kinematic_evaluator/README.md
Original file line number Diff line number Diff line change
@@ -1,3 +1,7 @@
# Kinematic Evaluator

TBD

## Parameters

{{json_to_markdown("evaluator/kinematic_evaluator/schema/kinematic_evaluator.schema.json")}}
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
{
"$schema": "http://json-schema.org/draft-07/schema#",
"title": "Parameters for kinematic_evaluator_node",
"type": "object",
"definitions": {
"kinematic_evaluator_node": {
"type": "object",
"properties": {
"output_file": {
"type": "string",
"default": "kinematic_metrics.results",
"description": "Name of the file to which kinematic metrics are written. If empty, metrics are not written to a file."
},
"selected_metrics": {
"type": "string",
"default": "velocity_stats",
"description": "The specific metrics selected for evaluation."
}
},
"required": ["output_file", "selected_metrics"]
}
},
"properties": {
"/**": {
"type": "object",
"properties": {
"ros__parameters": {
"$ref": "#/definitions/kinematic_evaluator_node"
}
},
"required": ["ros__parameters"]
}
},
"required": ["/**"]
}
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@

#include <lanelet2_core/LaneletMap.h>
#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_core/geometry/LaneletMap.h>
#include <lanelet2_core/geometry/Point.h>
#include <lanelet2_routing/RoutingGraph.h>
#include <tf2/utils.h>
Expand Down Expand Up @@ -360,20 +361,11 @@ CrosswalkEdgePoints getCrosswalkEdgePoints(const lanelet::ConstLanelet & crosswa
}

bool withinRoadLanelet(
const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr,
const TrackedObject & object,
const std::vector<std::pair<double, lanelet::Lanelet>> & surrounding_lanelets_with_dist,
const bool use_yaw_information = false)
{
const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position;
lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y);
// nearest lanelet
constexpr double search_radius = 10.0; // [m]
const auto surrounding_lanelets =
lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, search_radius);

for (const auto & lanelet_with_dist : surrounding_lanelets) {
const auto & dist = lanelet_with_dist.first;
const auto & lanelet = lanelet_with_dist.second;

for (const auto & [dist, lanelet] : surrounding_lanelets_with_dist) {
if (lanelet.hasAttribute(lanelet::AttributeName::Subtype)) {
lanelet::Attribute attr = lanelet.attribute(lanelet::AttributeName::Subtype);
if (
Expand All @@ -397,6 +389,20 @@ bool withinRoadLanelet(
return false;
}

bool withinRoadLanelet(
const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr,
const bool use_yaw_information = false)
{
const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position;
lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y);
// nearest lanelet
constexpr double search_radius = 10.0; // [m]
const auto surrounding_lanelets_with_dist =
lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, search_point, search_radius);

return withinRoadLanelet(object, surrounding_lanelets_with_dist, use_yaw_information);
}

boost::optional<CrosswalkEdgePoints> isReachableCrosswalkEdgePoints(
const TrackedObject & object, const lanelet::ConstLanelets & surrounding_lanelets,
const lanelet::ConstLanelets & surrounding_crosswalks, const CrosswalkEdgePoints & edge_points,
Expand Down Expand Up @@ -1411,9 +1417,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear;
const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y);
const auto velocity = std::max(min_crosswalk_user_velocity_, estimated_velocity);
// TODO(Mamoru Sobue): 3rd argument of findNearest is the number of lanelets, not radius, so past
// implementation has been wrong.
const auto surrounding_lanelets_with_dist = lanelet::geometry::findNearest(
const auto surrounding_lanelets_with_dist = lanelet::geometry::findWithin2d(
lanelet_map_ptr_->laneletLayer, lanelet::BasicPoint2d{obj_pos.x, obj_pos.y},
prediction_time_horizon_.pedestrian * velocity);
lanelet::ConstLanelets surrounding_lanelets;
Expand Down Expand Up @@ -1458,7 +1462,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(

// If the object is not crossing the crosswalk, in the road lanelets, try to find the closest
// crosswalk and generate path to the crosswalk edge
} else if (withinRoadLanelet(object, lanelet_map_ptr_)) {
} else if (withinRoadLanelet(object, surrounding_lanelets_with_dist)) {
lanelet::ConstLanelet closest_crosswalk{};
const auto & obj_pose = object.kinematics.pose_with_covariance.pose;
const auto found_closest_crosswalk =
Expand Down
2 changes: 2 additions & 0 deletions planning/autoware_freespace_planner/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,8 @@ None

### Parameters

{{json_to_markdown("planning/autoware_freespace_planner/schema/freespace_planner.schema.json")}}

#### Node parameters

| Parameter | Type | Description |
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,232 @@
{
"$schema": "http://json-schema.org/draft-07/schema#",
"title": "Parameters for freespace_planner",
"type": "object",
"definitions": {
"freespace_planner_params": {
"type": "object",
"properties": {
"planning_algorithm": {
"type": "string",
"enum": ["astar", "rrtstar"],
"default": "astar",
"description": "Planning algorithm to use, options: astar, rrtstar."
},
"waypoints_velocity": {
"type": "number",
"default": 5.0,
"description": "velocity in output trajectory (currently, only constant velocity is supported."
},
"update_rate": {
"type": "number",
"default": 10.0,
"description": "timer's update rate"
},
"th_arrived_distance_m": {
"type": "number",
"default": 1.0,
"description": "Threshold for considering the vehicle has arrived at its goal."
},
"th_stopped_time_sec": {
"type": "number",
"default": 1.0,
"description": "Time threshold for considering the vehicle as stopped."
},
"th_stopped_velocity_mps": {
"type": "number",
"default": 0.01,
"description": "Velocity threshold for considering the vehicle as stopped."
},
"th_course_out_distance_m": {
"type": "number",
"default": 1.0,
"description": "Threshold distance for considering the vehicle has deviated from its course."
},
"vehicle_shape_margin_m": {
"type": "number",
"default": 1.0,
"description": "Margin around the vehicle shape for planning."
},
"replan_when_obstacle_found": {
"type": "boolean",
"default": true,
"description": "Replan path when an obstacle is found."
},
"replan_when_course_out": {
"type": "boolean",
"default": true,
"description": "Replan path when the vehicle deviates from its course."
},
"time_limit": {
"type": "number",
"default": 30000.0,
"description": "Time limit for the planner."
},
"max_turning_ratio": {
"type": "number",
"default": 0.5,
"description": "Maximum turning ratio, relative to the actual turning limit of the vehicle."
},
"turning_steps": {
"type": "number",
"default": 1,
"description": "Number of steps for turning."
},
"theta_size": {
"type": "number",
"default": 144,
"description": "Number of discretized angles for search."
},
"angle_goal_range": {
"type": "number",
"default": 6.0,
"description": "Range of acceptable angles at the goal."
},
"lateral_goal_range": {
"type": "number",
"default": 0.5,
"description": "Lateral range of acceptable goal positions."
},
"longitudinal_goal_range": {
"type": "number",
"default": 2.0,
"description": "Longitudinal range of acceptable goal positions."
},
"curve_weight": {
"type": "number",
"default": 0.5,
"description": "Weight for curves in the cost function."
},
"reverse_weight": {
"type": "number",
"default": 1.0,
"description": "Weight for reverse movement in the cost function."
},
"direction_change_weight": {
"type": "number",
"default": 1.5,
"description": "Weight for direction changes in the cost function."
},
"obstacle_threshold": {
"type": "number",
"default": 100,
"description": "Threshold for considering an obstacle in the costmap."
},
"astar": {
"type": "object",
"properties": {
"only_behind_solutions": {
"type": "boolean",
"default": false,
"description": "Consider only solutions behind the vehicle."
},
"use_back": {
"type": "boolean",
"default": true,
"description": "Allow reverse motion in A* search."
},
"expansion_distance": {
"type": "number",
"default": 0.5,
"description": "Distance for expanding nodes in A* search."
},
"distance_heuristic_weight": {
"type": "number",
"default": 1.0,
"description": "Weight for the distance heuristic in A* search."
},
"smoothness_weight": {
"type": "number",
"default": 0.5,
"description": "Weight for the smoothness heuristic in A* search."
}
},
"required": [
"only_behind_solutions",
"use_back",
"expansion_distance",
"distance_heuristic_weight",
"smoothness_weight"
]
},
"rrtstar": {
"type": "object",
"properties": {
"enable_update": {
"type": "boolean",
"default": true,
"description": "Enable updates in RRT*."
},
"use_informed_sampling": {
"type": "boolean",
"default": true,
"description": "Use informed sampling in RRT*."
},
"max_planning_time": {
"type": "number",
"default": 150.0,
"description": "Maximum planning time for RRT*."
},
"neighbor_radius": {
"type": "number",
"default": 8.0,
"description": "Radius for neighboring nodes in RRT*."
},
"margin": {
"type": "number",
"default": 0.1,
"description": "Margin for RRT* sampling."
}
},
"required": [
"enable_update",
"use_informed_sampling",
"max_planning_time",
"neighbor_radius",
"margin"
]
}
},
"required": [
"planning_algorithm",
"waypoints_velocity",
"update_rate",
"th_arrived_distance_m",
"th_stopped_time_sec",
"th_stopped_velocity_mps",
"th_course_out_distance_m",
"vehicle_shape_margin_m",
"replan_when_obstacle_found",
"replan_when_course_out",
"time_limit",
"max_turning_ratio",
"turning_steps",
"theta_size",
"angle_goal_range",
"lateral_goal_range",
"longitudinal_goal_range",
"curve_weight",
"reverse_weight",
"direction_change_weight",
"obstacle_threshold",
"astar",
"rrtstar"
],
"additionalProperties": false
}
},
"properties": {
"/**": {
"type": "object",
"properties": {
"ros__parameters": {
"$ref": "#/definitions/freespace_planner_params"
}
},
"required": ["ros__parameters"],
"additionalProperties": false
}
},
"required": ["/**"],
"additionalProperties": false
}
3 changes: 3 additions & 0 deletions planning/autoware_freespace_planning_algorithms/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -23,10 +23,13 @@
<depend>nlohmann-json-dev</depend>
<depend>pybind11_vendor</depend>
<depend>rosbag2_cpp</depend>
<depend>rosbag2_storage</depend>
<depend>std_msgs</depend>
<depend>tf2</depend>
<depend>tf2_geometry_msgs</depend>

<exec_depend>rosbag2_storage_mcap</exec_depend>

<test_depend>ament_cmake_ros</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
# Develop purpose maps

## road_shoulders

The road_shoulders lanelet map consist of a variety of pick-up/drop-off site maps with road_shoulder tags including:

- pick-up/drop-off sites on the side of street lanes
- pick-up/drop-off sites on the side of curved lanes
- pick-up/drop-off sites inside a private area

![road_shoulder_test](./road_shoulder_test_map.png)
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading

0 comments on commit 9b6b2ac

Please sign in to comment.