Skip to content

Commit

Permalink
pre-commit run -a
Browse files Browse the repository at this point in the history
Signed-off-by: xtk8532704 <[email protected]>
  • Loading branch information
xtk8532704 committed Aug 16, 2024
1 parent 9f77a51 commit 52db00d
Show file tree
Hide file tree
Showing 3 changed files with 21 additions and 29 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -184,15 +184,16 @@ class PlanningEvaluatorNode : public rclcpp::Node
// queue for diagnostics and time stamp
std::deque<std::pair<DiagnosticStatus, rclcpp::Time>> diag_queue_;
const std::vector<std::string> target_functions_ = {
"obstacle_cruise_planner_stop", "obstacle_cruise_planner_slow_down",
"obstacle_cruise_planner_stop",
"obstacle_cruise_planner_slow_down",
"obstacle_cruise_planner_cruise",
"autoware::motion_velocity_planner::OutOfLaneModule.stop",
"autoware::motion_velocity_planner::OutOfLaneModule.slow_down",
"autoware::motion_velocity_planner::ObstacleVelocityLimiterModule.stop",
"autoware::motion_velocity_planner::ObstacleVelocityLimiterModule.slow_down",
"autoware::motion_velocity_planner::DynamicObstacleStopModule.stop",
"autoware::motion_velocity_planner::DynamicObstacleStopModule.slow_down",
};
};
std::optional<AccelWithCovarianceStamped> prev_acc_stamped_{std::nullopt};
};
} // namespace planning_diagnostics
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -72,8 +72,7 @@ void MotionVelocityPlannerManager::update_module_parameters(
}

std::shared_ptr<DiagnosticStatus> MotionVelocityPlannerManager::make_diagnostic(
const std::string & reason,
const bool is_decided)
const std::string & reason, const bool is_decided)
{
auto status = std::make_shared<DiagnosticStatus>();
status->level = status->OK;
Expand All @@ -92,25 +91,19 @@ std::shared_ptr<DiagnosticStatus> MotionVelocityPlannerManager::make_diagnostic(
return status;
}


void MotionVelocityPlannerManager::publish_diagnostics(
const rclcpp::Publisher<DiagnosticArray>::SharedPtr pub_ptr,
const rclcpp::Time & current_time,
const bool publish_decided_diagnostics_only
) const
const rclcpp::Publisher<DiagnosticArray>::SharedPtr pub_ptr, const rclcpp::Time & current_time,
const bool publish_decided_diagnostics_only) const
{

if (publish_decided_diagnostics_only &&
!std::any_of(
diagnostics_.begin(),
diagnostics_.end(),
[](const auto& ds_ptr) {
return ds_ptr && !ds_ptr->values.empty() && ds_ptr->values[0].key == "decision" && ds_ptr->values[0].value != "none";
}
)) {
if (
publish_decided_diagnostics_only &&
!std::any_of(diagnostics_.begin(), diagnostics_.end(), [](const auto & ds_ptr) {
return ds_ptr && !ds_ptr->values.empty() && ds_ptr->values[0].key == "decision" &&
ds_ptr->values[0].value != "none";
})) {
return;
}

DiagnosticArray diagnostics;
diagnostics.header.stamp = current_time;
diagnostics.header.frame_id = "map";
Expand All @@ -122,20 +115,21 @@ void MotionVelocityPlannerManager::publish_diagnostics(
pub_ptr->publish(diagnostics);
}


std::vector<VelocityPlanningResult> MotionVelocityPlannerManager::plan_velocities(
const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
const std::shared_ptr<const PlannerData> planner_data)
{
std::vector<VelocityPlanningResult> results;
for (auto & plugin : loaded_plugins_){
for (auto & plugin : loaded_plugins_) {
VelocityPlanningResult res = plugin->plan(ego_trajectory_points, planner_data);
results.push_back(res);

auto stop_reason_diag = make_diagnostic(plugin->get_module_name()+".stop", res.stop_points.size() > 0);
auto stop_reason_diag =
make_diagnostic(plugin->get_module_name() + ".stop", res.stop_points.size() > 0);
diagnostics_.push_back(stop_reason_diag);

auto slow_down_reason_diag = make_diagnostic(plugin->get_module_name()+".slow_down", res.slowdown_intervals.size() > 0);
auto slow_down_reason_diag =
make_diagnostic(plugin->get_module_name() + ".slow_down", res.slowdown_intervals.size() > 0);
diagnostics_.push_back(slow_down_reason_diag);
}
return results;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -54,14 +54,11 @@ class MotionVelocityPlannerManager

// Diagnostic
std::shared_ptr<DiagnosticStatus> make_diagnostic(
const std::string & reason,
const bool is_decided = true);
const std::string & reason, const bool is_decided = true);
void publish_diagnostics(
const rclcpp::Publisher<DiagnosticArray>::SharedPtr pub_ptr,
const rclcpp::Time & current_time,
const rclcpp::Publisher<DiagnosticArray>::SharedPtr pub_ptr, const rclcpp::Time & current_time,
const bool publish_decided_diagnostics_only = true) const;
void clear_diagnostics(){diagnostics_.clear();}

void clear_diagnostics() { diagnostics_.clear(); }

private:
std::vector<std::shared_ptr<DiagnosticStatus>> diagnostics_;
Expand Down

0 comments on commit 52db00d

Please sign in to comment.