Skip to content

Commit

Permalink
temp
Browse files Browse the repository at this point in the history 
Signed-off-by: beyza <[email protected]>
  • Loading branch information
@beyzanurkaya
beyzanurkaya committed Aug 12, 2024
1 parent f6fcfd9 commit 7dac190
Show file tree
Hide file tree
Showing 2 changed files with 164 additions and 39 deletions.
202 changes: 163 additions & 39 deletions ...avior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,7 +64,7 @@ MinMaxValue combineMinMaxValues(const MinMaxValue & r1, const MinMaxValue & r2)
return MinMaxValue{std::min(r1.min_value, r2.min_value), std::max(r1.max_value, r2.max_value)};
}

void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Pose & obj_pose)
[[maybe_unused]]void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Pose & obj_pose)
{
auto marker = autoware::universe_utils::createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), "dynamic_objects_to_avoid",
Expand All @@ -76,7 +76,7 @@ void appendObjectMarker(MarkerArray & marker_array, const geometry_msgs::msg::Po
marker_array.markers.push_back(marker);
}

void appendExtractedPolygonMarker(
[[maybe_unused]]void appendExtractedPolygonMarker(
MarkerArray & marker_array, const autoware::universe_utils::Polygon2d & obj_poly,
const double obj_z)
{
Expand Down Expand Up @@ -339,10 +339,12 @@ DynamicObstacleAvoidanceModule::DynamicObstacleAvoidanceModule(
parameters_->successive_num_to_entry_dynamic_avoidance_condition,
parameters_->successive_num_to_exit_dynamic_avoidance_condition)}
{

}

bool DynamicObstacleAvoidanceModule::isExecutionRequested() const
{
std::cout << "DynamicObstacleAvoidanceModule::isExecutionRequested()" << std::endl;
RCLCPP_DEBUG(getLogger(), "DYNAMIC AVOIDANCE isExecutionRequested.");

const auto input_path = getPreviousModuleOutput().path;
Expand All @@ -361,6 +363,7 @@ bool DynamicObstacleAvoidanceModule::isExecutionRequested() const
return true;
}


// check if there is target objects to avoid
return !target_objects_.empty();
}
Expand All @@ -373,6 +376,7 @@ bool DynamicObstacleAvoidanceModule::isExecutionReady() const

void DynamicObstacleAvoidanceModule::updateData()
{
std::cout << "DynamicObstacleAvoidanceModule::updateData()" << std::endl;
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

info_marker_.markers.clear();
Expand All @@ -392,6 +396,59 @@ void DynamicObstacleAvoidanceModule::updateData()
determineWhetherToAvoidAgainstRegulatedObjects(prev_objects);
determineWhetherToAvoidAgainstUnregulatedObjects(prev_objects);

// generate drivable lanes
std::vector<DrivableAreaInfo::Obstacle> obstacles_for_drivable_area;

// generate drivable lanes
auto current_lanelets = utils::dynamic_obstacle_avoidance::getCurrentLanesFromPath(
getPreviousModuleOutput().reference_path, planner_data_);
// std::for_each(
// current_lanelets.begin(), current_lanelets.end(), [&](const auto & lanelet) {
// current_drivable_area_info.drivable_lanes.push_back(
// utils::dynamic_obstacle_avoidance::generateExpandedDrivableLanes(
// lanelet, planner_data_, parameters_));
// });




DrivableAreaInfo current_drivable_area_info;
BehaviorModuleOutput output;
output.path = getPreviousModuleOutput().path;
output.drivable_area_info = utils::combineDrivableAreaInfo(
current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
output.reference_path = getPreviousModuleOutput().reference_path;
output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
output.modified_goal = getPreviousModuleOutput().modified_goal;

{

// generate drivable lanes
std::for_each(
current_lanelets.begin(), current_lanelets.end(), [&](const auto & lanelet) {
current_drivable_area_info.drivable_lanes.push_back(
utils::dynamic_obstacle_avoidance::generateExpandedDrivableLanes(
lanelet, planner_data_, parameters_));
});
// expand hatched road markings
current_drivable_area_info.enable_expanding_hatched_road_markings =
parameters_->use_hatched_road_markings;
// expand intersection areas
current_drivable_area_info.enable_expanding_intersection_areas =
parameters_->use_intersection_areas;
// expand freespace areas
current_drivable_area_info.enable_expanding_freespace_areas = parameters_->use_freespace_areas;
// generate obstacle polygons
current_drivable_area_info.obstacles.clear();
current_drivable_area_info.obstacles = obstacles_for_drivable_area;


output.drivable_area_info = utils::combineDrivableAreaInfo(
current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);

setDrivableLanes(output.drivable_area_info.drivable_lanes);
}

const auto target_objects_candidate = target_objects_manager_.getValidObjects();
target_objects_.clear();
for (const auto & target_object_candidate : target_objects_candidate) {
Expand All @@ -408,6 +465,7 @@ bool DynamicObstacleAvoidanceModule::canTransitSuccessState()

BehaviorModuleOutput DynamicObstacleAvoidanceModule::plan()
{
std::cout << "DynamicObstacleAvoidanceModule::plan()" << std::endl;
const auto & input_path = getPreviousModuleOutput().path;
if (input_path.points.empty()) {
throw std::runtime_error("input path is empty");
Expand All @@ -417,46 +475,21 @@ BehaviorModuleOutput DynamicObstacleAvoidanceModule::plan()

// create obstacles to avoid (= extract from the drivable area)
std::vector<DrivableAreaInfo::Obstacle> obstacles_for_drivable_area;
for (const auto & object : target_objects_) {
const auto obstacle_poly = [&]() {
if (getObjectType(object.label) == ObjectType::UNREGULATED) {
return calcPredictedPathBasedDynamicObstaclePolygon(object, ego_path_reserve_poly);
}

if (parameters_->polygon_generation_method == PolygonGenerationMethod::EGO_PATH_BASE) {
return calcEgoPathBasedDynamicObstaclePolygon(object);
}
if (parameters_->polygon_generation_method == PolygonGenerationMethod::OBJECT_PATH_BASE) {
return calcObjectPathBasedDynamicObstaclePolygon(object);
}
throw std::logic_error("The polygon_generation_method's string is invalid.");
}();
if (obstacle_poly) {
// obstacles_for_drivable_area.push_back(
// {object.pose, obstacle_poly.value(), object.is_collision_left});

appendObjectMarker(info_marker_, object.pose);
appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value(), object.pose.position.z);
}
}

DrivableAreaInfo current_drivable_area_info;

// generate drivable lanes
auto current_lanelets = utils::dynamic_obstacle_avoidance::getCurrentLanesFromPath(
getPreviousModuleOutput().reference_path, planner_data_);
std::for_each(current_lanelets.begin(), current_lanelets.end(), [&](const auto & lanelet) {
current_drivable_area_info.drivable_lanes.push_back(
utils::dynamic_obstacle_avoidance::generateExpandedDrivableLanes(
lanelet, planner_data_, parameters_));
});
// std::for_each(
// current_lanelets.begin(), current_lanelets.end(), [&](const auto & lanelet) {
// current_drivable_area_info.drivable_lanes.push_back(
// utils::dynamic_obstacle_avoidance::generateExpandedDrivableLanes(
// lanelet, planner_data_, parameters_));
// });

// current_drivable_area_info.drivable_lanes =
// getPreviousModuleOutput().drivable_area_info.drivable_lanes;
current_drivable_area_info.obstacles = obstacles_for_drivable_area;
current_drivable_area_info.enable_expanding_hatched_road_markings =
parameters_->use_hatched_road_markings;



DrivableAreaInfo current_drivable_area_info;
BehaviorModuleOutput output;
output.path = input_path;
output.drivable_area_info = utils::combineDrivableAreaInfo(
Expand All @@ -465,6 +498,56 @@ BehaviorModuleOutput DynamicObstacleAvoidanceModule::plan()
output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
output.modified_goal = getPreviousModuleOutput().modified_goal;

{

// generate drivable lanes
std::for_each(
current_lanelets.begin(), current_lanelets.end(), [&](const auto & lanelet) {
current_drivable_area_info.drivable_lanes.push_back(
utils::dynamic_obstacle_avoidance::generateExpandedDrivableLanes(
lanelet, planner_data_, parameters_));
});
// expand hatched road markings
current_drivable_area_info.enable_expanding_hatched_road_markings =
parameters_->use_hatched_road_markings;
// expand intersection areas
current_drivable_area_info.enable_expanding_intersection_areas =
parameters_->use_intersection_areas;
// expand freespace areas
current_drivable_area_info.enable_expanding_freespace_areas = parameters_->use_freespace_areas;
// generate obstacle polygons
current_drivable_area_info.obstacles.clear();

for (const auto & object : target_objects_) {
const auto obstacle_poly = [&]() {
if (getObjectType(object.label) == ObjectType::UNREGULATED) {
return calcPredictedPathBasedDynamicObstaclePolygon(object, ego_path_reserve_poly);
}

if (parameters_->polygon_generation_method == PolygonGenerationMethod::EGO_PATH_BASE) {
return calcEgoPathBasedDynamicObstaclePolygon(object);
}
if (parameters_->polygon_generation_method == PolygonGenerationMethod::OBJECT_PATH_BASE) {
return calcObjectPathBasedDynamicObstaclePolygon(object);
}
throw std::logic_error("The polygon_generation_method's string is invalid.");
}();
if (obstacle_poly) {
obstacles_for_drivable_area.push_back(
{object.pose, obstacle_poly.value(), object.is_collision_left});

appendObjectMarker(info_marker_, object.pose);
appendExtractedPolygonMarker(debug_marker_, obstacle_poly.value(), object.pose.position.z);
}
}
current_drivable_area_info.obstacles = obstacles_for_drivable_area;


output.drivable_area_info = utils::combineDrivableAreaInfo(
current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);

setDrivableLanes(output.drivable_area_info.drivable_lanes);
}
return output;
}

Expand All @@ -476,10 +559,51 @@ CandidateOutput DynamicObstacleAvoidanceModule::planCandidate() const

BehaviorModuleOutput DynamicObstacleAvoidanceModule::planWaitingApproval()
{
BehaviorModuleOutput out = plan();
return out;
}
auto current_lanelets = utils::dynamic_obstacle_avoidance::getCurrentLanesFromPath(
getPreviousModuleOutput().reference_path, planner_data_);
// std::for_each(
// current_lanelets.begin(), current_lanelets.end(), [&](const auto & lanelet) {
// current_drivable_area_info.drivable_lanes.push_back(
// utils::dynamic_obstacle_avoidance::generateExpandedDrivableLanes(
// lanelet, planner_data_, parameters_));
// });

DrivableAreaInfo current_drivable_area_info;
BehaviorModuleOutput output;
output.path = getPreviousModuleOutput().path;
output.drivable_area_info = utils::combineDrivableAreaInfo(
current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);
output.reference_path = getPreviousModuleOutput().reference_path;
output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
output.modified_goal = getPreviousModuleOutput().modified_goal;

{
// generate drivable lanes
std::for_each(current_lanelets.begin(), current_lanelets.end(), [&](const auto & lanelet) {
current_drivable_area_info.drivable_lanes.push_back(
utils::dynamic_obstacle_avoidance::generateExpandedDrivableLanes(
lanelet, planner_data_, parameters_));
});
// expand hatched road markings
current_drivable_area_info.enable_expanding_hatched_road_markings =
parameters_->use_hatched_road_markings;
// expand intersection areas
current_drivable_area_info.enable_expanding_intersection_areas =
parameters_->use_intersection_areas;
// expand freespace areas
current_drivable_area_info.enable_expanding_freespace_areas = parameters_->use_freespace_areas;
// generate obstacle polygons
current_drivable_area_info.obstacles.clear();

output.drivable_area_info = utils::combineDrivableAreaInfo(
current_drivable_area_info, getPreviousModuleOutput().drivable_area_info);

setDrivableLanes(output.drivable_area_info.drivable_lanes);
BehaviorModuleOutput out = plan();

return out;
}
}
ObjectType DynamicObstacleAvoidanceModule::getObjectType(const uint8_t label) const
{
using autoware_perception_msgs::msg::ObjectClassification;
Expand Down
1 change: 1 addition & 0 deletions ...avior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2329,6 +2329,7 @@ DrivableLanes generateExpandedDrivableLanes(
const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<DynamicAvoidanceParameters> & parameters)
{
std::cout << "generateExpandedDrivableLanes" << std::endl;
const auto & route_handler = planner_data->route_handler;

DrivableLanes current_drivable_lanes;
Expand Down

0 comments on commit 7dac190

Please sign in to comment.