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(autoware_behavior_velocity_planner_common): replace autoware_universe_utils with autoware_utils #10154

Open
wants to merge 7 commits into
base: main
Choose a base branch
from
+219 −227
Open

feat(autoware_behavior_velocity_planner_common): replace autoware_universe_utils with autoware_utils #10154

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
6becff7
feat(autoware_behavior_velocity_planner_common): replace autoware_uni…
mitsudome-r Feb 18, 2025
1aa726e
style(pre-commit): autofix
pre-commit-ci[bot] Feb 18, 2025
d1c221a
fix: fix build error
mitsudome-r Feb 19, 2025
d0890ac
style(pre-commit): autofix
pre-commit-ci[bot] Feb 19, 2025
c048113
feat(autoware_velocity_smoother): replace autoware_universe_utils wit…
mitsudome-r Feb 19, 2025
7a25e5c
fix: fix build error
mitsudome-r Feb 19, 2025
631f8f2
style(pre-commit): autofix
pre-commit-ci[bot] Feb 19, 2025
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
41 changes: 20 additions & 21 deletions planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,14 +18,6 @@
#include "autoware/motion_utils/trajectory/conversion.hpp"
#include "autoware/motion_utils/trajectory/trajectory.hpp"
#include "autoware/osqp_interface/osqp_interface.hpp"
#include "autoware/universe_utils/geometry/geometry.hpp"
#include "autoware/universe_utils/math/unit_conversion.hpp"
#include "autoware/universe_utils/ros/diagnostics_interface.hpp"
#include "autoware/universe_utils/ros/logger_level_configure.hpp"
#include "autoware/universe_utils/ros/polling_subscriber.hpp"
#include "autoware/universe_utils/ros/self_pose_listener.hpp"
#include "autoware/universe_utils/system/stop_watch.hpp"
#include "autoware/universe_utils/system/time_keeper.hpp"
#include "autoware/velocity_smoother/resample.hpp"
#include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp"
#include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp"
Expand All @@ -37,7 +29,15 @@
#include "tf2/utils.h"
#include "tf2_ros/transform_listener.h"

#include <autoware/universe_utils/ros/published_time_publisher.hpp>
#include <autoware_utils/geometry/geometry.hpp>
#include <autoware_utils/math/unit_conversion.hpp>
#include <autoware_utils/ros/diagnostics_interface.hpp>
#include <autoware_utils/ros/logger_level_configure.hpp>
#include <autoware_utils/ros/polling_subscriber.hpp>
#include <autoware_utils/ros/published_time_publisher.hpp>
#include <autoware_utils/ros/self_pose_listener.hpp>
#include <autoware_utils/system/stop_watch.hpp>
#include <autoware_utils/system/time_keeper.hpp>

#include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp"
#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp"
Expand All @@ -61,10 +61,10 @@ namespace autoware::velocity_smoother
using autoware_planning_msgs::msg::Trajectory;
using autoware_planning_msgs::msg::TrajectoryPoint;
using TrajectoryPoints = std::vector<TrajectoryPoint>;
using autoware::universe_utils::DiagnosticsInterface;
using autoware_adapi_v1_msgs::msg::OperationModeState;
using autoware_internal_debug_msgs::msg::Float32Stamped;
using autoware_internal_debug_msgs::msg::Float64Stamped;
using autoware_utils::DiagnosticsInterface;
using geometry_msgs::msg::AccelWithCovarianceStamped;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::PoseStamped;
Expand All @@ -90,14 +90,14 @@ class VelocitySmootherNode : public rclcpp::Node
rclcpp::Publisher<Trajectory>::SharedPtr pub_trajectory_;
rclcpp::Publisher<MarkerArray>::SharedPtr pub_virtual_wall_;
rclcpp::Subscription<Trajectory>::SharedPtr sub_current_trajectory_;
autoware::universe_utils::InterProcessPollingSubscriber<Odometry> sub_current_odometry_{
autoware_utils::InterProcessPollingSubscriber<Odometry> sub_current_odometry_{
this, "/localization/kinematic_state"};
autoware::universe_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped>
autoware_utils::InterProcessPollingSubscriber<AccelWithCovarianceStamped>
sub_current_acceleration_{this, "~/input/acceleration"};
autoware::universe_utils::InterProcessPollingSubscriber<
VelocityLimit, universe_utils::polling_policy::Newest>
autoware_utils::InterProcessPollingSubscriber<
VelocityLimit, autoware_utils::polling_policy::Newest>
sub_external_velocity_limit_{this, "~/input/external_velocity_limit_mps"};
autoware::universe_utils::InterProcessPollingSubscriber<OperationModeState> sub_operation_mode_{
autoware_utils::InterProcessPollingSubscriber<OperationModeState> sub_operation_mode_{
this, "~/input/operation_mode_state"};

Odometry::ConstSharedPtr current_odometry_ptr_; // current odometry
Expand Down Expand Up @@ -255,7 +255,7 @@ class VelocitySmootherNode : public rclcpp::Node
void initCommonParam();

// debug
autoware::universe_utils::StopWatch<std::chrono::milliseconds> stop_watch_;
autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch_;
std::shared_ptr<rclcpp::Time> prev_time_;
double prev_acc_;
rclcpp::Publisher<Float32Stamped>::SharedPtr pub_dist_to_stopline_;
Expand All @@ -270,8 +270,7 @@ class VelocitySmootherNode : public rclcpp::Node
rclcpp::Publisher<Float32Stamped>::SharedPtr debug_closest_jerk_;
rclcpp::Publisher<Float64Stamped>::SharedPtr debug_calculation_time_;
rclcpp::Publisher<Float32Stamped>::SharedPtr debug_closest_max_velocity_;
rclcpp::Publisher<autoware::universe_utils::ProcessingTimeDetail>::SharedPtr
debug_processing_time_detail_;
rclcpp::Publisher<autoware_utils::ProcessingTimeDetail>::SharedPtr debug_processing_time_detail_;

// For Jerk Filtered Algorithm Debug
rclcpp::Publisher<Trajectory>::SharedPtr pub_forward_filtered_trajectory_;
Expand All @@ -285,10 +284,10 @@ class VelocitySmootherNode : public rclcpp::Node
void flipVelocity(TrajectoryPoints & points) const;
void publishStopWatchTime();

std::unique_ptr<autoware::universe_utils::LoggerLevelConfigure> logger_configure_;
std::unique_ptr<autoware::universe_utils::PublishedTimePublisher> published_time_publisher_;
std::unique_ptr<autoware_utils::LoggerLevelConfigure> logger_configure_;
std::unique_ptr<autoware_utils::PublishedTimePublisher> published_time_publisher_;

mutable std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper_{nullptr};
mutable std::shared_ptr<autoware_utils::TimeKeeper> time_keeper_{nullptr};

std::unique_ptr<DiagnosticsInterface> diagnostics_interface_{nullptr};
};
Expand Down
6 changes: 3 additions & 3 deletions ...er/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,9 +16,9 @@
#define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT

#include "autoware/motion_utils/trajectory/trajectory.hpp"
#include "autoware/universe_utils/system/time_keeper.hpp"
#include "autoware/velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp"
#include "autoware/velocity_smoother/smoother/smoother_base.hpp"
#include "autoware_utils/system/time_keeper.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/utils.h"

Expand Down Expand Up @@ -68,8 +68,8 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase
};

explicit AnalyticalJerkConstrainedSmoother(
rclcpp::Node & node, const std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper =
std::make_shared<autoware::universe_utils::TimeKeeper>());
rclcpp::Node & node, const std::shared_ptr<autoware_utils::TimeKeeper> time_keeper =
std::make_shared<autoware_utils::TimeKeeper>());

bool apply(
const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
Expand Down
6 changes: 3 additions & 3 deletions ..._velocity_smoother/include/autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,9 +17,9 @@

#include "autoware/motion_utils/trajectory/trajectory.hpp"
#include "autoware/qp_interface/qp_interface.hpp"
#include "autoware/universe_utils/geometry/geometry.hpp"
#include "autoware/universe_utils/system/time_keeper.hpp"
#include "autoware/velocity_smoother/smoother/smoother_base.hpp"
#include "autoware_utils/geometry/geometry.hpp"
#include "autoware_utils/system/time_keeper.hpp"

#include "autoware_planning_msgs/msg/trajectory_point.hpp"

Expand All @@ -43,7 +43,7 @@ class JerkFilteredSmoother : public SmootherBase
};

explicit JerkFilteredSmoother(
rclcpp::Node & node, const std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper);
rclcpp::Node & node, const std::shared_ptr<autoware_utils::TimeKeeper> time_keeper);

bool apply(
const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
Expand Down
6 changes: 3 additions & 3 deletions ...velocity_smoother/include/autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,9 +17,9 @@

#include "autoware/motion_utils/trajectory/trajectory.hpp"
#include "autoware/osqp_interface/osqp_interface.hpp"
#include "autoware/universe_utils/geometry/geometry.hpp"
#include "autoware/universe_utils/system/time_keeper.hpp"
#include "autoware/velocity_smoother/smoother/smoother_base.hpp"
#include "autoware_utils/geometry/geometry.hpp"
#include "autoware_utils/system/time_keeper.hpp"

#include "autoware_planning_msgs/msg/trajectory_point.hpp"

Expand All @@ -41,7 +41,7 @@ class L2PseudoJerkSmoother : public SmootherBase
};

explicit L2PseudoJerkSmoother(
rclcpp::Node & node, const std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper);
rclcpp::Node & node, const std::shared_ptr<autoware_utils::TimeKeeper> time_keeper);

bool apply(
const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
Expand Down
6 changes: 3 additions & 3 deletions ...locity_smoother/include/autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,9 +17,9 @@

#include "autoware/motion_utils/trajectory/trajectory.hpp"
#include "autoware/osqp_interface/osqp_interface.hpp"
#include "autoware/universe_utils/geometry/geometry.hpp"
#include "autoware/universe_utils/system/time_keeper.hpp"
#include "autoware/velocity_smoother/smoother/smoother_base.hpp"
#include "autoware_utils/geometry/geometry.hpp"
#include "autoware_utils/system/time_keeper.hpp"

#include "autoware_planning_msgs/msg/trajectory_point.hpp"

Expand All @@ -41,7 +41,7 @@ class LinfPseudoJerkSmoother : public SmootherBase
};

explicit LinfPseudoJerkSmoother(
rclcpp::Node & node, const std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper);
rclcpp::Node & node, const std::shared_ptr<autoware_utils::TimeKeeper> time_keeper);

bool apply(
const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
Expand Down
6 changes: 3 additions & 3 deletions .../autoware_velocity_smoother/include/autoware/velocity_smoother/smoother/smoother_base.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,8 @@
#ifndef AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_
#define AUTOWARE__VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_

#include "autoware/universe_utils/system/time_keeper.hpp"
#include "autoware/velocity_smoother/resample.hpp"
#include "autoware_utils/system/time_keeper.hpp"
#include "rclcpp/rclcpp.hpp"

#include "autoware_planning_msgs/msg/trajectory_point.hpp"
Expand Down Expand Up @@ -57,7 +57,7 @@ class SmootherBase
};

explicit SmootherBase(
rclcpp::Node & node, const std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper);
rclcpp::Node & node, const std::shared_ptr<autoware_utils::TimeKeeper> time_keeper);
virtual ~SmootherBase() = default;
virtual bool apply(
const double initial_vel, const double initial_acc, const TrajectoryPoints & input,
Expand Down Expand Up @@ -91,7 +91,7 @@ class SmootherBase

protected:
BaseParam base_param_;
mutable std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper_{nullptr};
mutable std::shared_ptr<autoware_utils::TimeKeeper> time_keeper_{nullptr};
};
} // namespace autoware::velocity_smoother

Expand Down
2 changes: 1 addition & 1 deletion planning/autoware_velocity_smoother/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@
<depend>autoware_planning_msgs</depend>
<depend>autoware_planning_test_manager</depend>
<depend>autoware_qp_interface</depend>
<depend>autoware_universe_utils</depend>
<depend>autoware_utils</depend>
<depend>autoware_vehicle_info_utils</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
Expand Down
47 changes: 22 additions & 25 deletions planning/autoware_velocity_smoother/src/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,10 +15,10 @@
#include "autoware/velocity_smoother/node.hpp"

#include "autoware/motion_utils/marker/marker_helper.hpp"
#include "autoware/universe_utils/ros/update_param.hpp"
#include "autoware/velocity_smoother/smoother/jerk_filtered_smoother.hpp"
#include "autoware/velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp"
#include "autoware/velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp"
#include "autoware_utils/ros/update_param.hpp"

#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>

Expand Down Expand Up @@ -50,10 +50,9 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti

// create time_keeper and its publisher
// NOTE: This has to be called before setupSmoother to pass the time_keeper to the smoother.
debug_processing_time_detail_ = create_publisher<autoware::universe_utils::ProcessingTimeDetail>(
"~/debug/processing_time_detail_ms", 1);
time_keeper_ =
std::make_shared<autoware::universe_utils::TimeKeeper>(debug_processing_time_detail_);
debug_processing_time_detail_ =
create_publisher<autoware_utils::ProcessingTimeDetail>("~/debug/processing_time_detail_ms", 1);
time_keeper_ = std::make_shared<autoware_utils::TimeKeeper>(debug_processing_time_detail_);

// create smoother
setupSmoother(wheelbase_);
Expand Down Expand Up @@ -98,9 +97,8 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti

clock_ = get_clock();

logger_configure_ = std::make_unique<autoware::universe_utils::LoggerLevelConfigure>(this);
published_time_publisher_ =
std::make_unique<autoware::universe_utils::PublishedTimePublisher>(this);
logger_configure_ = std::make_unique<autoware_utils::LoggerLevelConfigure>(this);
published_time_publisher_ = std::make_unique<autoware_utils::PublishedTimePublisher>(this);
}

void VelocitySmootherNode::setupSmoother(const double wheelbase)
Expand Down Expand Up @@ -317,7 +315,7 @@ void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory

void VelocitySmootherNode::calcExternalVelocityLimit()
{
autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_);

if (!external_velocity_limit_ptr_) {
external_velocity_limit_.acceleration_request.request = false;
Expand Down Expand Up @@ -439,7 +437,7 @@ bool VelocitySmootherNode::checkData() const

void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr msg)
{
autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_);

RCLCPP_DEBUG(get_logger(), "========================= run start =========================");
stop_watch_.tic();
Expand All @@ -448,10 +446,10 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr
base_traj_raw_ptr_ = msg;

// receive data
current_odometry_ptr_ = sub_current_odometry_.takeData();
current_acceleration_ptr_ = sub_current_acceleration_.takeData();
external_velocity_limit_ptr_ = sub_external_velocity_limit_.takeData();
const auto operation_mode_ptr = sub_operation_mode_.takeData();
current_odometry_ptr_ = sub_current_odometry_.take_data();
current_acceleration_ptr_ = sub_current_acceleration_.take_data();
external_velocity_limit_ptr_ = sub_external_velocity_limit_.take_data();
const auto operation_mode_ptr = sub_operation_mode_.take_data();
if (operation_mode_ptr) {
operation_mode_ = *operation_mode_ptr;
}
Expand Down Expand Up @@ -534,7 +532,7 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr

void VelocitySmootherNode::updateDataForExternalVelocityLimit()
{
autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_);

if (prev_output_.empty()) {
return;
Expand All @@ -553,7 +551,7 @@ void VelocitySmootherNode::updateDataForExternalVelocityLimit()
TrajectoryPoints VelocitySmootherNode::calcTrajectoryVelocity(
const TrajectoryPoints & traj_input) const
{
autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_);

TrajectoryPoints output{}; // velocity is optimized by qp solver

Expand Down Expand Up @@ -602,7 +600,7 @@ bool VelocitySmootherNode::smoothVelocity(
const TrajectoryPoints & input, const size_t input_closest,
TrajectoryPoints & traj_smoothed) const
{
autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_);

if (input.empty()) {
return false; // cannot apply smoothing
Expand Down Expand Up @@ -721,7 +719,7 @@ bool VelocitySmootherNode::smoothVelocity(
void VelocitySmootherNode::insertBehindVelocity(
const size_t output_closest, const InitializeType type, TrajectoryPoints & output) const
{
autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_);

const bool keep_closest_vel_for_behind =
(type == InitializeType::EGO_VELOCITY || type == InitializeType::LARGE_DEVIATION_REPLAN ||
Expand Down Expand Up @@ -785,7 +783,7 @@ void VelocitySmootherNode::publishStopDistance(const TrajectoryPoints & trajecto
std::pair<Motion, VelocitySmootherNode::InitializeType> VelocitySmootherNode::calcInitialMotion(
const TrajectoryPoints & input_traj, const size_t input_closest) const
{
autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_);

const double vehicle_speed = std::fabs(current_odometry_ptr_->twist.twist.linear.x);
const double vehicle_acceleration = current_acceleration_ptr_->accel.accel.linear.x;
Expand Down Expand Up @@ -873,7 +871,7 @@ std::pair<Motion, VelocitySmootherNode::InitializeType> VelocitySmootherNode::ca
void VelocitySmootherNode::overwriteStopPoint(
const TrajectoryPoints & input, TrajectoryPoints & output) const
{
autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_);

const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex(input);
if (!stop_idx) {
Expand Down Expand Up @@ -917,7 +915,7 @@ void VelocitySmootherNode::overwriteStopPoint(

void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) const
{
autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_);

if (traj.size() < 1) {
return;
Expand Down Expand Up @@ -958,15 +956,15 @@ void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) c

void VelocitySmootherNode::applyStopApproachingVelocity(TrajectoryPoints & traj) const
{
autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_);

const auto stop_idx = autoware::motion_utils::searchZeroVelocityIndex(traj);
if (!stop_idx) {
return; // no stop point.
}
double distance_sum = 0.0;
for (size_t i = *stop_idx - 1; i < traj.size(); --i) { // search backward
distance_sum += autoware::universe_utils::calcDistance2d(traj.at(i), traj.at(i + 1));
distance_sum += autoware_utils::calc_distance2d(traj.at(i), traj.at(i + 1));
if (distance_sum > node_param_.stopping_distance) {
break;
}
Expand Down Expand Up @@ -1075,8 +1073,7 @@ double VelocitySmootherNode::calcTravelDistance() const
const auto closest_point = calcProjectedTrajectoryPointFromEgo(prev_output_);

if (prev_closest_point_) {
const double travel_dist =
autoware::universe_utils::calcDistance2d(*prev_closest_point_, closest_point);
const double travel_dist = autoware_utils::calc_distance2d(*prev_closest_point_, closest_point);
return travel_dist;
}

Expand Down
Loading
Loading