Skip to content

Commit

Permalink
fix(behavior_velocity_planner): separate walkway manager
Browse files Browse the repository at this point in the history 
Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
  • Loading branch information
@rej55
rej55 committed Jun 21, 2022
1 parent 1822a00 commit 3b181ac
Show file tree
Hide file tree
Showing 3 changed files with 92 additions and 51 deletions.
23 changes: 14 additions & 9 deletions planning/behavior_velocity_planner/include/scene_module/crosswalk/manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,17 +39,22 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC

private:
CrosswalkModule::PlannerParam crosswalk_planner_param_;
WalkwayModule::PlannerParam walkway_planner_param_;

std::vector<lanelet::ConstLanelet> getCrosswalksOnPath(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const lanelet::LaneletMapPtr lanelet_map,
const std::shared_ptr<const lanelet::routing::RoutingGraphContainer> & overall_graphs);
void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;

std::function<bool(const std::shared_ptr<SceneModuleInterface> &)> getModuleExpiredFunction(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;
};

class WalkwayModuleManager : public SceneModuleManagerInterface
{
public:
explicit WalkwayModuleManager(rclcpp::Node & node);

const char * getModuleName() override { return "walkway"; }

std::set<int64_t> getCrosswalkIdSetOnPath(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const lanelet::LaneletMapPtr lanelet_map,
const std::shared_ptr<const lanelet::routing::RoutingGraphContainer> & overall_graphs);
private:
WalkwayModule::PlannerParam walkway_planner_param_;

void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override;

Expand Down
1 change: 1 addition & 0 deletions planning/behavior_velocity_planner/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -166,6 +166,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
// Initialize PlannerManager
if (this->declare_parameter("launch_crosswalk", true)) {
planner_manager_.launchSceneModule(std::make_shared<CrosswalkModuleManager>(*this));
planner_manager_.launchSceneModule(std::make_shared<WalkwayModuleManager>(*this));
}
if (this->declare_parameter("launch_traffic_light", true)) {
planner_manager_.launchSceneModule(std::make_shared<TrafficLightModuleManager>(*this));
Expand Down
119 changes: 77 additions & 42 deletions planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,32 +19,10 @@
#include <string>
#include <vector>

namespace behavior_velocity_planner
namespace
{
CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
: SceneModuleManagerInterfaceWithRTC(node, getModuleName())
{
const std::string ns(getModuleName());

// for crosswalk parameters
auto & cp = crosswalk_planner_param_;
cp.stop_line_distance = node.declare_parameter(ns + ".crosswalk.stop_line_distance", 1.5);
cp.stop_margin = node.declare_parameter(ns + ".crosswalk.stop_margin", 1.0);
cp.slow_margin = node.declare_parameter(ns + ".crosswalk.slow_margin", 2.0);
cp.slow_velocity = node.declare_parameter(ns + ".crosswalk.slow_velocity", 5.0 / 3.6);
cp.stop_predicted_object_prediction_time_margin =
node.declare_parameter(ns + ".crosswalk.stop_predicted_object_prediction_time_margin", 3.0);
cp.external_input_timeout = node.declare_parameter(ns + ".crosswalk.external_input_timeout", 1.0);

// for walkway parameters
auto & wp = walkway_planner_param_;
wp.stop_margin = node.declare_parameter(ns + ".walkway.stop_margin", 1.0);
wp.stop_line_distance = node.declare_parameter(ns + ".walkway.stop_line_distance", 1.0);
wp.stop_duration_sec = node.declare_parameter(ns + ".walkway.stop_duration_sec", 1.0);
wp.external_input_timeout = node.declare_parameter(ns + ".walkway.external_input_timeout", 1.0);
}

std::vector<lanelet::ConstLanelet> CrosswalkModuleManager::getCrosswalksOnPath(
std::vector<lanelet::ConstLanelet> getCrosswalksOnPath(
const geometry_msgs::msg::Pose & current_pose,
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const lanelet::LaneletMapPtr lanelet_map,
const std::shared_ptr<const lanelet::routing::RoutingGraphContainer> & overall_graphs)
Expand All @@ -53,14 +31,13 @@ std::vector<lanelet::ConstLanelet> CrosswalkModuleManager::getCrosswalksOnPath(
std::set<int64_t> unique_lane_ids;

auto nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex(
path.points, planner_data_->current_pose.pose, std::numeric_limits<double>::max(), M_PI_2);
path.points, current_pose, std::numeric_limits<double>::max(), M_PI_2);

// Add current lane id
lanelet::ConstLanelets current_lanes;
if (
lanelet::utils::query::getCurrentLanelets(
lanelet::utils::query::laneletLayer(lanelet_map), planner_data_->current_pose.pose,
&current_lanes) &&
lanelet::utils::query::laneletLayer(lanelet_map), current_pose, &current_lanes) &&
nearest_segment_idx) {
for (const auto & ll : current_lanes) {
if (
Expand Down Expand Up @@ -91,40 +68,55 @@ std::vector<lanelet::ConstLanelet> CrosswalkModuleManager::getCrosswalksOnPath(
return crosswalks;
}

std::set<int64_t> CrosswalkModuleManager::getCrosswalkIdSetOnPath(
std::set<int64_t> getCrosswalkIdSetOnPath(
const geometry_msgs::msg::Pose & current_pose,
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const lanelet::LaneletMapPtr lanelet_map,
const std::shared_ptr<const lanelet::routing::RoutingGraphContainer> & overall_graphs)
{
std::set<int64_t> crosswalk_id_set;

for (const auto & crosswalk : getCrosswalksOnPath(path, lanelet_map, overall_graphs)) {
for (const auto & crosswalk :
getCrosswalksOnPath(current_pose, path, lanelet_map, overall_graphs)) {
crosswalk_id_set.insert(crosswalk.id());
}

return crosswalk_id_set;
}
} // namespace

namespace behavior_velocity_planner
{
CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
: SceneModuleManagerInterfaceWithRTC(node, getModuleName())
{
const std::string ns(getModuleName());

// for crosswalk parameters
auto & cp = crosswalk_planner_param_;
cp.stop_line_distance = node.declare_parameter(ns + ".crosswalk.stop_line_distance", 1.5);
cp.stop_margin = node.declare_parameter(ns + ".crosswalk.stop_margin", 1.0);
cp.slow_margin = node.declare_parameter(ns + ".crosswalk.slow_margin", 2.0);
cp.slow_velocity = node.declare_parameter(ns + ".crosswalk.slow_velocity", 5.0 / 3.6);
cp.stop_predicted_object_prediction_time_margin =
node.declare_parameter(ns + ".crosswalk.stop_predicted_object_prediction_time_margin", 3.0);
cp.external_input_timeout = node.declare_parameter(ns + ".crosswalk.external_input_timeout", 1.0);
}

void CrosswalkModuleManager::launchNewModules(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
{
const auto rh = planner_data_->route_handler_;
for (const auto & crosswalk :
getCrosswalksOnPath(path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr())) {
for (const auto & crosswalk : getCrosswalksOnPath(
planner_data_->current_pose.pose, path, rh->getLaneletMapPtr(),
rh->getOverallGraphPtr())) {
const auto module_id = crosswalk.id();
if (!isModuleRegistered(module_id)) {
registerModule(std::make_shared<CrosswalkModule>(
module_id, crosswalk, crosswalk_planner_param_, logger_.get_child("crosswalk_module"),
clock_));
if (
crosswalk.attributeOr(lanelet::AttributeNamesString::Subtype, std::string("")) ==
lanelet::AttributeValueString::Walkway) {
registerModule(std::make_shared<WalkwayModule>(
module_id, crosswalk, walkway_planner_param_, logger_.get_child("walkway_module"),
clock_));
}
generateUUID(module_id);
}
generateUUID(module_id);
}
}

Expand All @@ -133,12 +125,55 @@ CrosswalkModuleManager::getModuleExpiredFunction(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
{
const auto rh = planner_data_->route_handler_;
const auto crosswalk_id_set =
getCrosswalkIdSetOnPath(path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr());
const auto crosswalk_id_set = getCrosswalkIdSetOnPath(
planner_data_->current_pose.pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr());

return [crosswalk_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
return crosswalk_id_set.count(scene_module->getModuleId()) == 0;
};
}

WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node)
: SceneModuleManagerInterface(node, getModuleName())
{
const std::string ns(getModuleName());

// for walkway parameters
auto & wp = walkway_planner_param_;
wp.stop_margin = node.declare_parameter(ns + ".walkway.stop_margin", 1.0);
wp.stop_line_distance = node.declare_parameter(ns + ".walkway.stop_line_distance", 1.0);
wp.stop_duration_sec = node.declare_parameter(ns + ".walkway.stop_duration_sec", 1.0);
wp.external_input_timeout = node.declare_parameter(ns + ".walkway.external_input_timeout", 1.0);
}

void WalkwayModuleManager::launchNewModules(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
{
const auto rh = planner_data_->route_handler_;
for (const auto & crosswalk : getCrosswalksOnPath(
planner_data_->current_pose.pose, path, rh->getLaneletMapPtr(),
rh->getOverallGraphPtr())) {
const auto module_id = crosswalk.id();
if (
!isModuleRegistered(module_id) &&
crosswalk.attributeOr(lanelet::AttributeNamesString::Subtype, std::string("")) ==
lanelet::AttributeValueString::Walkway) {
registerModule(std::make_shared<WalkwayModule>(
module_id, crosswalk, walkway_planner_param_, logger_.get_child("walkway_module"), clock_));
}
}
}

std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
WalkwayModuleManager::getModuleExpiredFunction(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
{
const auto rh = planner_data_->route_handler_;
const auto walkway_id_set = getCrosswalkIdSetOnPath(
planner_data_->current_pose.pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr());

return [walkway_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
return walkway_id_set.count(scene_module->getModuleId()) == 0;
};
}
} // namespace behavior_velocity_planner

0 comments on commit 3b181ac

Please sign in to comment.