Skip to content

Commit

Permalink
feat: add new score achievability
Browse files Browse the repository at this point in the history 
Signed-off-by: satoshi-ota <[email protected]>
  • Loading branch information
@satoshi-ota
satoshi-ota committed Sep 20, 2024
1 parent 281e23d commit c0e512b
Show file tree
Hide file tree
Showing 6 changed files with 292 additions and 43 deletions.
19 changes: 19 additions & 0 deletions planning/autoware_planning_data_analyzer/launch/behavior_analyzer.launch.xml
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,13 @@
<launch>
<arg name="bag_path" description="bagfile path"/>
<arg name="map_path" description="point cloud and lanelet2 map directory path"/>

<arg name="use_sim_time" default="false" description="use_sim_time"/>
<arg name="vehicle_model" default="sample_vehicle" description="vehicle model name"/>
<arg name="use_map_in_bag" default="false"/>

<arg name="lanelet2_map_loader_param_path" default="$(find-pkg-share autoware_launch)/config/map/lanelet2_map_loader.param.yaml"/>
<arg name="map_projection_loader_param_path" default="$(find-pkg-share autoware_launch)/config/map/map_projection_loader.param.yaml"/>

<arg name="input/odometry" default="/localization/kinematic_state"/>
<arg name="input/acceleration" default="/localization/acceleration"/>
Expand All @@ -21,8 +27,21 @@
<composable_node pkg="autoware_planning_data_analyzer" plugin="autoware::behavior_analyzer::BehaviorAnalyzerNode" name="behavior_analyzer">
<param name="bag_path" value="$(var bag_path)"/>
<param from="$(find-pkg-share autoware_planning_data_analyzer)/config/behavior_analyzer.param.yaml"/>
<remap from="input/lanelet2_map" to="/map/vector_map"/>
</composable_node>

<!-- <composable_node pkg="map_loader" plugin="Lanelet2MapLoaderNode" name="lanelet2_map_loader"> -->
<!-- <param from="$(var lanelet2_map_loader_param_path)"/> -->
<!-- <param name="lanelet2_map_path" value="$(var map_path)/lanelet2_map.osm"/> -->
<!-- <remap from="output/lanelet2_map" to="/map/vector_map"/> -->
<!-- </composable_node> -->

<composable_node pkg="glog_component" plugin="GlogComponent" name="glog_component" namespace=""/>
</node_container>

<!-- <include file="$(find-pkg-share autoware_map_projection_loader)/launch/map_projection_loader.launch.xml"> -->
<!-- <arg name="param_path" value="$(var map_projection_loader_param_path)"/> -->
<!-- <arg name="map_projector_info_path" value="$(var map_path)/map_projector_info.yaml"/> -->
<!-- <arg name="lanelet2_map_path" value="$(var map_path)/lanelet2_map.osm"/> -->
<!-- </include> -->
</launch>
1 change: 1 addition & 0 deletions planning/autoware_planning_data_analyzer/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
<buildtool_depend>eigen3_cmake_module</buildtool_depend>

<depend>autoware_frenet_planner</depend>
<depend>autoware_lanelet2_extension</depend>
<depend>autoware_map_msgs</depend>
<depend>autoware_motion_utils</depend>
<depend>autoware_path_sampler</depend>
Expand Down
66 changes: 62 additions & 4 deletions planning/autoware_planning_data_analyzer/src/data_structs.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@

#include "utils.hpp"

#include <autoware_lanelet2_extension/utility/utilities.hpp>

#include <algorithm>
#include <limits>
#include <memory>
Expand All @@ -33,7 +35,8 @@ std::string TOPIC::ACCELERATION = "/localization/acceleration"; // NO
std::string TOPIC::OBJECTS = "/perception/object_recognition/objects"; // NOLINT
std::string TOPIC::TRAJECTORY = "/planning/scenario_planning/trajectory"; // NOLINT
std::string TOPIC::STEERING = "/vehicle/status/steering_status"; // NOLINT
//
std::string TOPIC::ROUTE = "/planning/mission_planning/route"; // NOLINT

template <>
bool Buffer<SteeringReport>::ready() const
{
Expand Down Expand Up @@ -124,7 +127,10 @@ auto Buffer<TFMessage>::get(const rcutils_time_point_value_t now) const -> TFMes
CommonData::CommonData(
const std::shared_ptr<BagData> & bag_data, const vehicle_info_utils::VehicleInfo & vehicle_info,
const std::shared_ptr<Parameters> & parameters, const std::string & tag)
: vehicle_info{vehicle_info}, parameters{parameters}, tag{tag}
: vehicle_info{vehicle_info},
route_handler{bag_data->route_handler},
parameters{parameters},
tag{tag}
{
objects_history.reserve(parameters->resample_num);

Expand All @@ -149,31 +155,36 @@ void CommonData::calculate()
std::vector<double> minimum_ttc_values;
std::vector<double> longitudinal_jerk_values;
std::vector<double> travel_distance_values;
std::vector<double> lateral_deviation_values;

for (size_t i = 0; i < parameters->resample_num - 1; i++) {
lateral_accel_values.push_back(lateral_accel(i));
longitudinal_jerk_values.push_back(longitudinal_jerk(i));
minimum_ttc_values.push_back(minimum_ttc(i));
travel_distance_values.push_back(travel_distance(i));
lateral_deviation_values.push_back(lateral_deviation(i));
}

{
lateral_accel_values.push_back(lateral_accel(parameters->resample_num - 1));
longitudinal_jerk_values.push_back(0.0);
minimum_ttc_values.push_back(minimum_ttc(parameters->resample_num - 1));
travel_distance_values.push_back(travel_distance(parameters->resample_num - 1));
lateral_deviation_values.push_back(lateral_deviation(parameters->resample_num - 1));
}

values.at(static_cast<size_t>(METRIC::LATERAL_ACCEL)) = lateral_accel_values;
values.at(static_cast<size_t>(METRIC::LONGITUDINAL_JERK)) = longitudinal_jerk_values;
values.at(static_cast<size_t>(METRIC::MINIMUM_TTC)) = minimum_ttc_values;
values.at(static_cast<size_t>(METRIC::TRAVEL_DISTANCE)) = travel_distance_values;
values.at(static_cast<size_t>(METRIC::LATERAL_DEVIATION)) = lateral_deviation_values;

scores.at(static_cast<size_t>(SCORE::LATERAL_COMFORTABILITY)) = lateral_comfortability();
scores.at(static_cast<size_t>(SCORE::LONGITUDINAL_COMFORTABILITY)) =
longitudinal_comfortability();
scores.at(static_cast<size_t>(SCORE::EFFICIENCY)) = efficiency();
scores.at(static_cast<size_t>(SCORE::SAFETY)) = safety();
scores.at(static_cast<size_t>(SCORE::ACHIEVABILITY)) = achievability();
}

double CommonData::longitudinal_comfortability() const
Expand Down Expand Up @@ -259,12 +270,35 @@ double CommonData::safety() const
return score / parameters->resample_num;
}

double CommonData::total(const double w0, const double w1, const double w2, const double w3) const
double CommonData::achievability() const
{
constexpr double TIME_FACTOR = 1.0;

double score = 0.0;

const auto min = 0.0;
const auto max = 2.0;
const auto normalize = [&min, &max](const double value) {
return (max - std::clamp(value, min, max)) / (max - min);
};

for (size_t i = 0; i < parameters->resample_num; i++) {
score += normalize(
std::pow(TIME_FACTOR, i) *
std::abs(values.at(static_cast<size_t>(METRIC::LATERAL_DEVIATION)).at(i)));
}

return score / parameters->resample_num;
}

double CommonData::total(
const double w0, const double w1, const double w2, const double w3, const double w4) const
{
return w0 * scores.at(static_cast<size_t>(SCORE::LATERAL_COMFORTABILITY)) +
w1 * scores.at(static_cast<size_t>(SCORE::LONGITUDINAL_COMFORTABILITY)) +
w2 * scores.at(static_cast<size_t>(SCORE::EFFICIENCY)) +
w3 * scores.at(static_cast<size_t>(SCORE::SAFETY));
w3 * scores.at(static_cast<size_t>(SCORE::SAFETY)) +
w4 * scores.at(static_cast<size_t>(SCORE::ACHIEVABILITY));
}

ManualDrivingData::ManualDrivingData(
Expand Down Expand Up @@ -347,6 +381,18 @@ double ManualDrivingData::travel_distance(const size_t idx) const
return distance;
}

double ManualDrivingData::lateral_deviation(const size_t idx) const
{
lanelet::ConstLanelet nearest{};
if (!route_handler->getClosestPreferredLaneletWithinRoute(
odometry_history.at(idx)->pose.pose, &nearest)) {
return std::numeric_limits<double>::max();
}
const auto arc_coordinates =
lanelet::utils::getArcCoordinates({nearest}, odometry_history.at(idx)->pose.pose);
return arc_coordinates.distance;
}

bool ManualDrivingData::ready() const
{
if (objects_history.size() < parameters->resample_num) {
Expand Down Expand Up @@ -402,6 +448,18 @@ double TrajectoryData::travel_distance(const size_t idx) const
return autoware::motion_utils::calcSignedArcLength(points, 0L, idx);
}

double TrajectoryData::lateral_deviation(const size_t idx) const
{
lanelet::ConstLanelet nearest{};
if (!route_handler->getClosestPreferredLaneletWithinRoute(
autoware::universe_utils::getPose(points.at(idx)), &nearest)) {
return std::numeric_limits<double>::max();
}
const auto arc_coordinates =
lanelet::utils::getArcCoordinates({nearest}, autoware::universe_utils::getPose(points.at(idx)));
return arc_coordinates.distance;
}

bool TrajectoryData::feasible() const
{
const auto condition = [](const auto & p) { return p.longitudinal_velocity_mps > -1e-3; };
Expand Down
60 changes: 45 additions & 15 deletions planning/autoware_planning_data_analyzer/src/data_structs.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@ enum class METRIC {
LONGITUDINAL_JERK = 2,
TRAVEL_DISTANCE = 3,
MINIMUM_TTC = 4,
LATERAL_DEVIATION = 5,
SIZE
};

Expand All @@ -45,6 +46,7 @@ enum class SCORE {
LONGITUDINAL_COMFORTABILITY = 1,
EFFICIENCY = 2,
SAFETY = 3,
ACHIEVABILITY = 4,
SIZE
};

Expand All @@ -56,6 +58,7 @@ struct TOPIC
static std::string OBJECTS;
static std::string TRAJECTORY;
static std::string STEERING;
static std::string ROUTE;
};

struct FrenetPoint
Expand Down Expand Up @@ -91,21 +94,23 @@ struct Parameters
double w1{1.0};
double w2{1.0};
double w3{1.0};
double w4{1.0};
GridSearchParameters grid_seach{};
TargetStateParameters target_state{};
};

struct Result
{
Result(const double w0, const double w1, const double w2, const double w3)
: w0{w0}, w1{w1}, w2{w2}, w3{w3}
Result(const double w0, const double w1, const double w2, const double w3, const double w4)
: w0{w0}, w1{w1}, w2{w2}, w3{w3}, w4{w4}
{
}
double loss{0.0};
double w0{0.0};
double w1{0.0};
double w2{0.0};
double w3{0.0};
double w4{0.0};
};

struct BufferBase
Expand Down Expand Up @@ -177,7 +182,8 @@ auto Buffer<TFMessage>::get(const rcutils_time_point_value_t now) const -> TFMes

struct BagData
{
explicit BagData(const rcutils_time_point_value_t timestamp) : timestamp{timestamp}
explicit BagData(const rcutils_time_point_value_t timestamp)
: timestamp{timestamp}, route_handler{std::make_shared<RouteHandler>()}
{
buffers.emplace(TOPIC::TF, std::make_shared<Buffer<TFMessage>>());
buffers.emplace(TOPIC::ODOMETRY, std::make_shared<Buffer<Odometry>>());
Expand All @@ -187,9 +193,23 @@ struct BagData
buffers.emplace(TOPIC::STEERING, std::make_shared<Buffer<SteeringReport>>());
}

rcutils_time_point_value_t timestamp;

std::map<std::string, std::shared_ptr<BufferBase>> buffers{};

rcutils_time_point_value_t timestamp;
std::shared_ptr<RouteHandler> route_handler;

void set_map(const LaneletMapBin::ConstSharedPtr hdmap) { route_handler->setMap(*hdmap); }

void set_route(const LaneletRoute::ConstSharedPtr route)
{
route_handler->setRoute(*route);
std::cout << "IDs:";
for (const auto & lanelet : route_handler->getPreferredLanelets()) {
std::cout << lanelet.id() << ",";
}
std::cout << std::endl;
}

void update(const rcutils_time_point_value_t dt)
{
Expand Down Expand Up @@ -227,7 +247,12 @@ struct CommonData

double safety() const;

double total(const double w0, const double w1, const double w2, const double w3) const;
double achievability() const;

double total(
const double w0, const double w1, const double w2, const double w3, const double w4) const;

auto score() const -> std::vector<double> { return scores; }

virtual double lateral_accel(const size_t idx) const = 0;

Expand All @@ -237,6 +262,8 @@ struct CommonData

virtual double travel_distance(const size_t idx) const = 0;

virtual double lateral_deviation(const size_t idx) const = 0;

virtual bool feasible() const = 0;

virtual bool ready() const = 0;
Expand All @@ -247,12 +274,10 @@ struct CommonData

std::vector<double> scores;

// std::unordered_map<METRIC, std::vector<double>> values;

// std::unordered_map<SCORE, double> scores;

vehicle_info_utils::VehicleInfo vehicle_info;

std::shared_ptr<RouteHandler> route_handler;

std::shared_ptr<Parameters> parameters;

std::string tag{""};
Expand All @@ -272,6 +297,8 @@ struct ManualDrivingData : CommonData

double travel_distance(const size_t idx) const override;

double lateral_deviation(const size_t idx) const override;

bool feasible() const override { return true; }

bool ready() const override;
Expand All @@ -296,6 +323,8 @@ struct TrajectoryData : CommonData

double travel_distance(const size_t idx) const override;

double lateral_deviation(const size_t idx) const override;

bool feasible() const override;

bool ready() const override;
Expand All @@ -309,15 +338,15 @@ struct SamplingTrajectoryData
const std::shared_ptr<BagData> & bag_data, const vehicle_info_utils::VehicleInfo & vehicle_info,
const std::shared_ptr<Parameters> & parameters);

auto best(const double w0, const double w1, const double w2, const double w3) const
-> std::optional<TrajectoryData>
auto best(const double w0, const double w1, const double w2, const double w3, const double w4)
const -> std::optional<TrajectoryData>
{
auto sort_by_score = data;

std::sort(
sort_by_score.begin(), sort_by_score.end(),
[&w0, &w1, &w2, &w3](const auto & a, const auto & b) {
return a.total(w0, w1, w2, w3) > b.total(w0, w1, w2, w3);
[&w0, &w1, &w2, &w3, &w4](const auto & a, const auto & b) {
return a.total(w0, w1, w2, w3, w4) > b.total(w0, w1, w2, w3, w4);
});

const auto itr = std::remove_if(
Expand Down Expand Up @@ -353,9 +382,10 @@ struct DataSet
{
}

auto loss(const double w0, const double w1, const double w2, const double w3) const -> double
auto loss(const double w0, const double w1, const double w2, const double w3, const double w4)
const -> double
{
const auto best = sampling.best(w0, w1, w2, w3);
const auto best = sampling.best(w0, w1, w2, w3, w4);
if (!best.has_value()) {
throw std::logic_error("no found best trajectory.");
}
Expand Down
Loading

0 comments on commit c0e512b

Please sign in to comment.