Skip to content

Commit

Permalink
refactor
Browse files Browse the repository at this point in the history
Signed-off-by: Yuki Takagi <[email protected]>
  • Loading branch information
yuki-takagi-66 committed Jan 31, 2024
1 parent ea803bb commit 68c214b
Show file tree
Hide file tree
Showing 3 changed files with 28 additions and 12 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
use_vehicle_acceleration: false # whether to consider current vehicle acceleration when predicting paths or not
speed_limit_multiplier: 1.5 # When using vehicle acceleration. Set vehicle's maximum predicted speed as the legal speed limit in that lanelet times this value
acceleration_exponential_half_life: 2.5 # [s] When using vehicle acceleration. The decaying acceleration model considers that the current vehicle acceleration will be halved after this many seconds
use_crosswalk_signal: true
# parameter for shoulder lane prediction
prediction_time_horizon_rate_for_validate_shoulder_lane_length: 0.8

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -189,6 +189,8 @@ class MapBasedPredictionNode : public rclcpp::Node
double speed_limit_multiplier_;
double acceleration_exponential_half_life_;

bool use_crosswalk_signal_;

// Stop watch
StopWatch<std::chrono::milliseconds> stop_watch_;

Expand Down Expand Up @@ -259,6 +261,7 @@ class MapBasedPredictionNode : public rclcpp::Node
bool isDuplicated(
const PredictedPath & predicted_path, const std::vector<PredictedPath> & predicted_paths);
std::optional<lanelet::Id> getTrafficSignalId(const lanelet::ConstLanelet & way_lanelet);
std::optional<TrafficSignalElement> getTrafficSignalElement(const lanelet::Id & id);

visualization_msgs::msg::Marker getDebugMarker(
const TrackedObject & object, const Maneuver & maneuver, const size_t obj_num);
Expand Down
36 changes: 24 additions & 12 deletions perception/map_based_prediction/src/map_based_prediction_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -788,6 +788,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
acceleration_exponential_half_life_ =
declare_parameter<double>("acceleration_exponential_half_life");

use_crosswalk_signal_ = declare_parameter<bool>("use_crosswalk_signal");

path_generator_ = std::make_shared<PathGenerator>(
prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_,
min_crosswalk_user_velocity_);
Expand Down Expand Up @@ -1230,18 +1232,13 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
// try to find the edge points for all crosswalks and generate path to the crosswalk edge
for (const auto & crosswalk : crosswalks_) {
const auto crosswalk_signal_id_opt = getTrafficSignalId(crosswalk);
if (crosswalk_signal_id_opt.has_value()) {
// If the crosswalk has traffic light, do something.
if (traffic_signal_id_map_.count(crosswalk_signal_id_opt.value()) != 0) {
const auto & signal_elements =
traffic_signal_id_map_.at(crosswalk_signal_id_opt.value()).elements;
if (signal_elements.size() > 1) {
RCLCPP_ERROR(
get_logger(), "[Map Based Prediction]: Multiple TrafficSignalElement_ are received.");
} else if (
!signal_elements.empty() && signal_elements.front().color == TrafficSignalElement::RED) {
continue;
}
if (crosswalk_signal_id_opt.has_value() && use_crosswalk_signal_) {
const auto signal_element_opt = getTrafficSignalElement(crosswalk_signal_id_opt.value());
const auto signal_color =
signal_element_opt ? signal_element_opt.value().color : TrafficSignalElement::UNKNOWN;

if (signal_color == TrafficSignalElement::RED) {
continue;
}
}

Expand Down Expand Up @@ -2255,6 +2252,21 @@ std::optional<lanelet::Id> MapBasedPredictionNode::getTrafficSignalId(
return traffic_light_reg_elems.front()->id();
}

std::optional<TrafficSignalElement> MapBasedPredictionNode::getTrafficSignalElement(
const lanelet::Id & id)
{
if (traffic_signal_id_map_.count(id) != 0) {
const auto & signal_elements = traffic_signal_id_map_.at(id).elements;
if (signal_elements.size() > 1) {
RCLCPP_ERROR(
get_logger(), "[Map Based Prediction]: Multiple TrafficSignalElement_ are received.");
} else if (!signal_elements.empty()) {
return signal_elements.front();
}
}
return std::nullopt;
}

} // namespace map_based_prediction

#include <rclcpp_components/register_node_macro.hpp>
Expand Down

0 comments on commit 68c214b

Please sign in to comment.