Skip to content

Commit

Permalink
ci(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
@pre-commit-ci
pre-commit-ci[bot] authored and Berkay committed Jun 9, 2022
1 parent 1efc8c5 commit eb29786
Show file tree
Hide file tree
Showing 5 changed files with 54 additions and 60 deletions.
2 changes: 1 addition & 1 deletion map/map_loader/config/lanelet2_map_loader.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,4 +4,4 @@
latitude: 40.816617984672746 # Latitude of map_origin, using in UTM
longitude: 29.360491808334285 # Longitude of map_origin, using in UTM

center_line_resolution: 5.0 # [m]
center_line_resolution: 5.0 # [m]
32 changes: 16 additions & 16 deletions planning/motion_velocity_smoother/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,20 +103,20 @@ After the optimization, a resampling called `post resampling` is performed befor

### Output

| Name | Type | Description |
| -------------------------------------------------- |-------------------------------------------|-----------------------------------------------------------------------------------------------------------|
| `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Modified trajectory |
| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32` | Current external velocity limit [m/s] |
| `~/closest_velocity` | `std_msgs/Float32` | Planned velocity closest to ego base_link (for debug) |
| `~/closest_acceleration` | `std_msgs/Float32` | Planned acceleration closest to ego base_link (for debug) |
| `~/closest_jerk` | `std_msgs/Float32` | Planned jerk closest to ego base_link (for debug) |
| `~/debug/trajectory_raw` | `autoware_auto_planning_msgs/Trajectory` | Extracted trajectory (for debug) |
| `~/debug/trajectory_external_velocity_limited` | `autoware_auto_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) |
| `~/debug/trajectory_lateral_acc_filtered` | `autoware_auto_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) |
| `~/debug/trajectory_steering_rate_limited` | `autoware_auto_planning_msgs/Trajectory` | Steering angle rate limit filtered trajectory (for debug) |
| `~/debug/trajectory_time_resampled` | `autoware_auto_planning_msgs/Trajectory` | Time resampled trajectory (for debug) |
| `~/distance_to_stopline` | `std_msgs/Float32` | Distance to stop line from current ego pose (max 50 m) (for debug) |
| `~/stop_speed_exceeded` | `std_msgs/Bool` | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold |
| Name | Type | Description |
| -------------------------------------------------- | ---------------------------------------- | --------------------------------------------------------------------------------------------------------- |
| `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Modified trajectory |
| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32` | Current external velocity limit [m/s] |
| `~/closest_velocity` | `std_msgs/Float32` | Planned velocity closest to ego base_link (for debug) |
| `~/closest_acceleration` | `std_msgs/Float32` | Planned acceleration closest to ego base_link (for debug) |
| `~/closest_jerk` | `std_msgs/Float32` | Planned jerk closest to ego base_link (for debug) |
| `~/debug/trajectory_raw` | `autoware_auto_planning_msgs/Trajectory` | Extracted trajectory (for debug) |
| `~/debug/trajectory_external_velocity_limited` | `autoware_auto_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) |
| `~/debug/trajectory_lateral_acc_filtered` | `autoware_auto_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) |
| `~/debug/trajectory_steering_rate_limited` | `autoware_auto_planning_msgs/Trajectory` | Steering angle rate limit filtered trajectory (for debug) |
| `~/debug/trajectory_time_resampled` | `autoware_auto_planning_msgs/Trajectory` | Time resampled trajectory (for debug) |
| `~/distance_to_stopline` | `std_msgs/Float32` | Distance to stop line from current ego pose (max 50 m) (for debug) |
| `~/stop_speed_exceeded` | `std_msgs/Bool` | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold |

## Parameters

Expand Down Expand Up @@ -198,7 +198,7 @@ After the optimization, a resampling called `post resampling` is performed befor
### Steering angle rate parameters

| Name | Type | Description | Default value |
|:--------------------------| :------- |:-----------------------------------------|:--------------|
| :------------------------ | :------- | :--------------------------------------- | :------------ |
| `max_steering_angle_rate` | `double` | Maximum steering tire angle rate [rad/s] | 0.349066 |
| `resample_ds` | `double` | Distance between trajectory points [m] | 0.5 |
| `max_lookup_distance` | `double` | Maximum lookup distance [m] | 2.0 |
Expand Down Expand Up @@ -234,7 +234,7 @@ After the optimization, a resampling called `post resampling` is performed befor
### Others

| Name | Type | Description | Default value |
|:------------------------------| :------- |:--------------------------------------------------------------------------------------------------| :------------ |
| :---------------------------- | :------- | :------------------------------------------------------------------------------------------------ | :------------ |
| `over_stop_velocity_warn_thr` | `double` | Threshold to judge that the optimized velocity exceeds the input velocity on the stop point [m/s] | 1.389 |

<!-- Write parameters of this package.
Expand Down
8 changes: 3 additions & 5 deletions ...ning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,9 @@
#include "rclcpp/rclcpp.hpp"
#include "tier4_autoware_utils/geometry/geometry.hpp"
#include "tier4_autoware_utils/trajectory/trajectory.hpp"
#include "vehicle_info_util/vehicle_info_util.hpp"

#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
#include "vehicle_info_util/vehicle_info_util.hpp"

#include "boost/optional.hpp"

Expand Down Expand Up @@ -55,7 +55,7 @@ class SmootherBase
double min_lookup_dist;
double max_lookup_dist;
double steering_limit_sample_rate;
resampling::ResampleParam resample_param;
resampling::ResampleParam resample_param;
};

explicit SmootherBase(rclcpp::Node & node);
Expand All @@ -70,9 +70,7 @@ class SmootherBase
virtual boost::optional<TrajectoryPoints> applyLateralAccelerationFilter(
const TrajectoryPoints & input) const;

boost::optional<TrajectoryPoints> applySteeringRateLimit(
const TrajectoryPoints & input) const;

boost::optional<TrajectoryPoints> applySteeringRateLimit(const TrajectoryPoints & input) const;

double getMaxAccel() const;
double getMinDecel() const;
Expand Down
11 changes: 6 additions & 5 deletions planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp"
#include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp"
#include "tier4_autoware_utils/ros/update_param.hpp"

#include <vehicle_info_util/vehicle_info_util.hpp>

#include <algorithm>
Expand Down Expand Up @@ -75,12 +76,12 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
default:
throw std::domain_error("[MotionVelocitySmootherNode] invalid algorithm");
}
//Initialize the wheelbase
// Initialize the wheelbase
auto p = smoother_->getBaseParam();
p.wheel_base = wheelbase_;
smoother_->setParam(p);

// publishers, subscribers
// publishers, subscribers
pub_trajectory_ = create_publisher<Trajectory>("~/output/trajectory", 1);
pub_velocity_limit_ = create_publisher<VelocityLimit>(
"~/output/current_velocity_limit_mps", rclcpp::QoS{1}.transient_local());
Expand Down Expand Up @@ -517,14 +518,14 @@ bool MotionVelocitySmootherNode::smoothVelocity(
}

// Steering angle rate limit
const auto traj_steering_rate_limited = smoother_->applySteeringRateLimit(*traj_lateral_acc_filtered);
const auto traj_steering_rate_limited =
smoother_->applySteeringRateLimit(*traj_lateral_acc_filtered);
if (!traj_steering_rate_limited) {
RCLCPP_ERROR(get_logger(), "Fail to do traj_steering_rate_limited");

return false;
}
// RCLCPP_WARN(get_logger(), "AAAAAAAAA CREATED");

// RCLCPP_WARN(get_logger(), "AAAAAAAAA CREATED");

// Resample trajectory with ego-velocity based interval distance
const auto traj_pre_resampled_closest = tier4_autoware_utils::findNearestIndex(
Expand Down
61 changes: 28 additions & 33 deletions planning/motion_velocity_smoother/src/smoother/smoother_base.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,6 @@ namespace motion_velocity_smoother
{
using vehicle_info_util::VehicleInfoUtil;


SmootherBase::SmootherBase(rclcpp::Node & node)
{
auto & p = base_param_;
Expand Down Expand Up @@ -53,17 +52,17 @@ SmootherBase::SmootherBase(rclcpp::Node & node)
node.declare_parameter("sparse_min_interval_distance", 4.0);
}

void SmootherBase::setParam(const BaseParam & param) {base_param_ = param;}
void SmootherBase::setParam(const BaseParam & param) { base_param_ = param; }

SmootherBase::BaseParam SmootherBase::getBaseParam() const {return base_param_;}
SmootherBase::BaseParam SmootherBase::getBaseParam() const { return base_param_; }

double SmootherBase::getMaxAccel() const {return base_param_.max_accel;}
double SmootherBase::getMaxAccel() const { return base_param_.max_accel; }

double SmootherBase::getMinDecel() const {return base_param_.min_decel;}
double SmootherBase::getMinDecel() const { return base_param_.min_decel; }

double SmootherBase::getMaxJerk() const {return base_param_.max_jerk;}
double SmootherBase::getMaxJerk() const { return base_param_.max_jerk; }

double SmootherBase::getMinJerk() const {return base_param_.min_jerk;}
double SmootherBase::getMinJerk() const { return base_param_.min_jerk; }

boost::optional<TrajectoryPoints> SmootherBase::applyLateralAccelerationFilter(
const TrajectoryPoints & input) const
Expand Down Expand Up @@ -128,12 +127,10 @@ boost::optional<TrajectoryPoints> SmootherBase::applyLateralAccelerationFilter(
boost::optional<TrajectoryPoints> SmootherBase::applySteeringRateLimit(
const TrajectoryPoints & input) const
{
const size_t min_lookup_index =
std::max(
const size_t min_lookup_index = std::max(
static_cast<size_t>(base_param_.min_lookup_dist / base_param_.sample_ds),
static_cast<size_t>(1));
const size_t max_lookup_index =
std::max(
const size_t max_lookup_index = std::max(
static_cast<size_t>(base_param_.max_lookup_dist / base_param_.sample_ds),
static_cast<size_t>(1));

Expand All @@ -142,7 +139,8 @@ boost::optional<TrajectoryPoints> SmootherBase::applySteeringRateLimit(
}

if (input.size() < min_lookup_index + 1) {
return boost::optional<TrajectoryPoints>(input); // cannot calculate the desired velocity. do nothing.
return boost::optional<TrajectoryPoints>(
input); // cannot calculate the desired velocity. do nothing.
}

std::vector<double> out_arclength;
Expand All @@ -167,29 +165,27 @@ boost::optional<TrajectoryPoints> SmootherBase::applySteeringRateLimit(
*/

for (size_t i = 0; output->size() > i; i++) {

if (i < output->size() - 1) {
output->at(i).front_wheel_angle_rad =
static_cast<float>(std::atan(
(tf2::getYaw(output->at(i + 1).pose.orientation) -
tf2::getYaw(output->at(i).pose.orientation)) * base_param_.wheel_base /
(base_param_.sample_ds)));
output->at(i).front_wheel_angle_rad = static_cast<float>(std::atan(
(tf2::getYaw(output->at(i + 1).pose.orientation) -
tf2::getYaw(output->at(i).pose.orientation)) *
base_param_.wheel_base / (base_param_.sample_ds)));
} else {
output->at(i).front_wheel_angle_rad = 0.0;
}
}

for (size_t i = 0; output->size() > i; i++) {

const size_t lookup_index = std::min(
(std::max(
static_cast<size_t>(output->at(output->size() - 1 - i).longitudinal_velocity_mps /
base_param_.sample_ds), min_lookup_index)), max_lookup_index);
static_cast<size_t>(
output->at(output->size() - 1 - i).longitudinal_velocity_mps / base_param_.sample_ds),
min_lookup_index)),
max_lookup_index);
double ds = static_cast<double>(lookup_index) * base_param_.sample_ds;
double mean_vel = 0.0;

if (lookup_index - 1 < i) {

for (size_t k = 0; k <= lookup_index; k++) {
mean_vel += output->at(output->size() - 1 - i + k).longitudinal_velocity_mps;
}
Expand All @@ -200,23 +196,22 @@ boost::optional<TrajectoryPoints> SmootherBase::applySteeringRateLimit(
double dt_steering = std::max(
std::fabs(
(output->at(output->size() - 1 - i).front_wheel_angle_rad -
output->at(output->size() - 1 - i + lookup_index).front_wheel_angle_rad)) /
base_param_.max_steering_angle_rate, std::numeric_limits<double>::epsilon());
output->at(output->size() - 1 - i + lookup_index).front_wheel_angle_rad)) /
base_param_.max_steering_angle_rate,
std::numeric_limits<double>::epsilon());

if (dt_steering > dt) {
output->at(output->size() - 1 - i).longitudinal_velocity_mps = (ds / dt_steering) * 2.0 -
output->at(output->size() - 1 - i).longitudinal_velocity_mps =
(ds / dt_steering) * 2.0 -
output->at(output->size() - 1 - i + lookup_index).longitudinal_velocity_mps;
if (output->at(output->size() - 1 - i).longitudinal_velocity_mps <
base_param_.min_curve_velocity)
{
output->at(
output->size() - 1 -
i).longitudinal_velocity_mps = base_param_.min_curve_velocity;

if (
output->at(output->size() - 1 - i).longitudinal_velocity_mps <
base_param_.min_curve_velocity) {
output->at(output->size() - 1 - i).longitudinal_velocity_mps =
base_param_.min_curve_velocity;
}
}
}

}

return boost::optional<TrajectoryPoints>(output);
Expand Down

0 comments on commit eb29786

Please sign in to comment.