Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(behavior_velocity_traffic_light): do not overwrite unknown with past known color (#7044) #1305

Merged
merged 1 commit into from
May 20, 2024
Merged

feat(behavior_velocity_traffic_light): do not overwrite unknown with past known color (#7044) #1305

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions planning/behavior_velocity_planner/src/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -352,6 +352,7 @@ void BehaviorVelocityPlannerNode::onTrafficSignals(
planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp =
msg->stamp;
} else {
// if (1)the observation is not UNKNOWN or (2)the very first observation is UNKNOWN
planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = traffic_signal;
}
}
Expand Down
1 change: 1 addition & 0 deletions planning/behavior_velocity_traffic_light_module/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
<maintainer email="[email protected]">Satoshi Ota</maintainer>
<maintainer email="[email protected]">Tomoya Kimura</maintainer>
<maintainer email="[email protected]">Shumpei Wakabayashi</maintainer>
<maintainer email="[email protected]">Mamoru Sobue</maintainer>

<license>Apache License 2.0</license>

Expand Down
39 changes: 11 additions & 28 deletions planning/behavior_velocity_traffic_light_module/src/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ void TrafficLightModuleManager::modifyPathVelocity(
stop_reason_array.header.stamp = path->header.stamp;

first_stop_path_point_index_ = static_cast<int>(path->points.size() - 1);
first_ref_stop_path_point_index_ = static_cast<int>(path->points.size() - 1);
nearest_ref_stop_path_point_index_ = static_cast<int>(path->points.size() - 1);
for (const auto & scene_module : scene_modules_) {
tier4_planning_msgs::msg::StopReason stop_reason;
std::shared_ptr<TrafficLightModule> traffic_light_scene_module(
Expand All @@ -85,8 +85,8 @@ void TrafficLightModuleManager::modifyPathVelocity(
}
if (
traffic_light_scene_module->getFirstRefStopPathPointIndex() <
first_ref_stop_path_point_index_) {
first_ref_stop_path_point_index_ =
nearest_ref_stop_path_point_index_) {
nearest_ref_stop_path_point_index_ =
traffic_light_scene_module->getFirstRefStopPathPointIndex();
if (
traffic_light_scene_module->getTrafficLightModuleState() !=
Expand Down Expand Up @@ -126,14 +126,13 @@ void TrafficLightModuleManager::launchNewModules(

// Use lanelet_id to unregister module when the route is changed
const auto lane_id = traffic_light_reg_elem.second.id();
const auto module_id = lane_id;
if (!isModuleRegisteredFromExistingAssociatedModule(module_id)) {
if (!isModuleRegisteredFromExistingAssociatedModule(lane_id)) {
registerModule(std::make_shared<TrafficLightModule>(
module_id, lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second,
planner_param_, logger_.get_child("traffic_light_module"), clock_));
generateUUID(module_id);
lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_,
logger_.get_child("traffic_light_module"), clock_));
generateUUID(lane_id);
updateRTCStatus(
getUUID(module_id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
getUUID(lane_id), true, std::numeric_limits<double>::lowest(), path.header.stamp);
}
}
}
Expand All @@ -145,33 +144,17 @@ TrafficLightModuleManager::getModuleExpiredFunction(
const auto lanelet_id_set = planning_utils::getLaneletIdSetOnPath<TrafficLight>(
path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose);

return [this, lanelet_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
return [this, lanelet_id_set](
[[maybe_unused]] const std::shared_ptr<SceneModuleInterface> & scene_module) {
for (const auto & id : lanelet_id_set) {
if (isModuleRegisteredFromRegElement(id, scene_module->getModuleId())) {
if (isModuleRegisteredFromExistingAssociatedModule(id)) {
return false;
}
}
return true;
};
}

bool TrafficLightModuleManager::isModuleRegisteredFromRegElement(
const lanelet::Id & id, const size_t module_id) const
{
const auto lane = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(id);

const auto registered_lane =
planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(module_id);
for (const auto & registered_element : registered_lane.regulatoryElementsAs<TrafficLight>()) {
for (const auto & element : lane.regulatoryElementsAs<TrafficLight>()) {
if (hasSameTrafficLight(element, registered_element)) {
return true;
}
}
}
return false;
}

bool TrafficLightModuleManager::isModuleRegisteredFromExistingAssociatedModule(
const lanelet::Id & id) const
{
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_velocity_traffic_light_module/src/manager.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC
// Debug
rclcpp::Publisher<autoware_perception_msgs::msg::TrafficSignal>::SharedPtr pub_tl_state_;

std::optional<int> first_ref_stop_path_point_index_;
std::optional<int> nearest_ref_stop_path_point_index_;
};

class TrafficLightModulePlugin : public PluginWrapper<TrafficLightModuleManager>
Expand Down
9 changes: 4 additions & 5 deletions planning/behavior_velocity_traffic_light_module/src/scene.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -162,11 +162,10 @@ bool calcStopPointAndInsertIndex(
} // namespace

TrafficLightModule::TrafficLightModule(
const int64_t module_id, const int64_t lane_id,
const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane,
const PlannerParam & planner_param, const rclcpp::Logger logger,
const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem,
lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger,
const rclcpp::Clock::SharedPtr clock)
: SceneModuleInterface(module_id, logger, clock),
: SceneModuleInterface(lane_id, logger, clock),
lane_id_(lane_id),
traffic_light_reg_elem_(traffic_light_reg_elem),
lane_(lane),
Expand Down Expand Up @@ -365,7 +364,7 @@ bool TrafficLightModule::findValidTrafficSignal(TrafficSignalStamped & valid_tra
{
// get traffic signal associated with the regulatory element id
const auto traffic_signal_stamped_opt = planner_data_->getTrafficSignal(
traffic_light_reg_elem_.id(), true /* traffic light module keeps last observation */);
traffic_light_reg_elem_.id(), false /* traffic light module does not keep last observation */);
if (!traffic_signal_stamped_opt) {
RCLCPP_WARN_STREAM_ONCE(
logger_, "the traffic signal data associated with regulatory element id "
Expand Down
5 changes: 2 additions & 3 deletions planning/behavior_velocity_traffic_light_module/src/scene.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,9 +66,8 @@ class TrafficLightModule : public SceneModuleInterface

public:
TrafficLightModule(
const int64_t module_id, const int64_t lane_id,
const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane,
const PlannerParam & planner_param, const rclcpp::Logger logger,
const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem,
lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger,
const rclcpp::Clock::SharedPtr clock);

bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;
Expand Down
Loading