Skip to content

Commit

Permalink
odom history interval param added (autowarefoundation#567)
Browse files Browse the repository at this point in the history
  • Loading branch information
@brkay54
brkay54 committed Apr 20, 2022
1 parent 5a8bfee commit f569d71
Show file tree
Hide file tree
Showing 5 changed files with 111 additions and 95 deletions.
5 changes: 3 additions & 2 deletions control/control_performance_analysis/config/control_performance_analysis.param.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
/**:
ros__parameters:
# -- publishing period --
control_period: 0.08
double curvature_interval_length_: 5.0
control_period: 0.01
curvature_interval_length_: 5.0
odom_interval_: 3
4 changes: 3 additions & 1 deletion ...mance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ class ControlPerformanceAnalysisCore
// See https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
ControlPerformanceAnalysisCore();
ControlPerformanceAnalysisCore(double wheelbase, double curvature_interval_length);
ControlPerformanceAnalysisCore(double wheelbase, double curvature_interval_length, uint odom_interval);

// Setters
void setCurrentPose(const Pose & msg);
Expand Down Expand Up @@ -84,6 +84,7 @@ class ControlPerformanceAnalysisCore
private:
double wheelbase_;
double curvature_interval_length_;
uint odom_interval_;

// Variables Received Outside
std::shared_ptr<PoseArray> current_waypoints_ptr_;
Expand All @@ -98,6 +99,7 @@ class ControlPerformanceAnalysisCore

std_msgs::msg::Header last_odom_header;


// Variables computed

std::unique_ptr<int32_t> idx_prev_wp_; // the waypoint index, vehicle
Expand Down
2 changes: 2 additions & 0 deletions ...mance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,8 @@ struct Param
// LPF1d Gain

double lpf_gain;

uint odom_interval;
};

class ControlPerformanceAnalysisNode : public rclcpp::Node
Expand Down
192 changes: 101 additions & 91 deletions control/control_performance_analysis/src/control_performance_analysis_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,16 +24,20 @@ namespace control_performance_analysis
{
using geometry_msgs::msg::Quaternion;

ControlPerformanceAnalysisCore::ControlPerformanceAnalysisCore() : wheelbase_{2.74}
ControlPerformanceAnalysisCore::ControlPerformanceAnalysisCore()
: wheelbase_{2.74}
{
prev_target_vars_ = std::make_unique<msg::ErrorStamped>();
current_velocities_ptr_ = std::make_shared<std::vector<double>>(2, 0.0);
odom_history_ptr_ = std::make_shared<std::vector<Odometry>>();
odom_interval_ = 0;
curvature_interval_length_ = 10.0;
}

ControlPerformanceAnalysisCore::ControlPerformanceAnalysisCore(
double wheelbase, double curvature_interval_length)
: wheelbase_{wheelbase}, curvature_interval_length_{curvature_interval_length}
double wheelbase, double curvature_interval_length, uint odom_interval)
: wheelbase_{wheelbase}, curvature_interval_length_{curvature_interval_length},
odom_interval_{odom_interval}
{
// prepare control performance struct
prev_target_vars_ = std::make_unique<msg::ErrorStamped>();
Expand All @@ -58,8 +62,8 @@ void ControlPerformanceAnalysisCore::setOdomHistory(const Odometry & odom)
if (!odom_history_ptr_->empty() && odom.header.stamp == odom_history_ptr_->back().header.stamp) {
return;
}
if (odom_history_ptr_->size() >= 3) {
// If higher than 3, remove the first element of vector
if (odom_history_ptr_->size() >= (3 + odom_interval_ * 2)) {
// If higher, remove the first element of vector
odom_history_ptr_->erase(odom_history_ptr_->begin());
odom_history_ptr_->push_back(odom);
} else {
Expand Down Expand Up @@ -95,24 +99,24 @@ std::pair<bool, int32_t> ControlPerformanceAnalysisCore::findClosestPrevWayPoint
std::vector<double> projection_distances_ds; //

auto f_projection_dist = [this](auto pose_1, auto pose_0) {
// Vector of intervals.
std::vector<double> int_vec{
pose_1.position.x - pose_0.position.x, pose_1.position.y - pose_0.position.y};
// Vector of intervals.
std::vector<double> int_vec{
pose_1.position.x - pose_0.position.x, pose_1.position.y - pose_0.position.y};

// Compute the magnitude of path interval vector
double ds_mag = std::hypot(int_vec[0], int_vec[1]);
// Compute the magnitude of path interval vector
double ds_mag = std::hypot(int_vec[0], int_vec[1]);

// Vector to vehicle from the origin waypoints.
std::vector<double> vehicle_vec{
this->current_vec_pose_ptr_->position.x - pose_0.position.x,
this->current_vec_pose_ptr_->position.y - pose_0.position.y};
// Vector to vehicle from the origin waypoints.
std::vector<double> vehicle_vec{
this->current_vec_pose_ptr_->position.x - pose_0.position.x,
this->current_vec_pose_ptr_->position.y - pose_0.position.y};

double projection_distance_onto_interval;
projection_distance_onto_interval =
(int_vec[0] * vehicle_vec[0] + int_vec[1] * vehicle_vec[1]) / ds_mag;
double projection_distance_onto_interval;
projection_distance_onto_interval =
(int_vec[0] * vehicle_vec[0] + int_vec[1] * vehicle_vec[1]) / ds_mag;

return projection_distance_onto_interval;
};
return projection_distance_onto_interval;
};

// Fill the projection_distances vector.
std::transform(
Expand All @@ -122,8 +126,8 @@ std::pair<bool, int32_t> ControlPerformanceAnalysisCore::findClosestPrevWayPoint

// Lambda function to replace negative numbers with a large number.
auto fnc_check_if_negative = [](auto x) {
return x < 0 ? std::numeric_limits<double>::max() : x;
};
return x < 0 ? std::numeric_limits<double>::max() : x;
};

std::vector<double> projections_distances_all_positive;
std::transform(
Expand All @@ -147,9 +151,9 @@ std::pair<bool, int32_t> ControlPerformanceAnalysisCore::findClosestPrevWayPoint
findCurveRefIdx();

return ((min_distance_ds <= acceptable_min_distance) & (*idx_prev_wp_ >= 0) &
(*idx_prev_wp_ <= length_of_trajectory))
? std::make_pair(true, *idx_prev_wp_)
: std::make_pair(false, std::numeric_limits<int32_t>::quiet_NaN());
(*idx_prev_wp_ <= length_of_trajectory)) ?
std::make_pair(true, *idx_prev_wp_) :
std::make_pair(false, std::numeric_limits<int32_t>::quiet_NaN());
}

bool ControlPerformanceAnalysisCore::isDataReady() const
Expand Down Expand Up @@ -264,67 +268,73 @@ bool ControlPerformanceAnalysisCore::calculateErrorVars()
bool ControlPerformanceAnalysisCore::calculateDrivingVars()
{
if (!odom_history_ptr_->empty()) {

int odom_size = odom_history_ptr_->size();

if (last_odom_header.stamp != odom_history_ptr_->at(odom_size - 1).header.stamp) {
// Calculate lateral acceleration
if(odom_history_ptr_->at(odom_size - 1).header.stamp != last_odom_header.stamp)
{


// Calculate lateral acceleration

driving_status_vars.driving_monitor.lateral_acceleration.header.set__stamp(
odom_history_ptr_->at(odom_size - 1).header.stamp);
driving_status_vars.driving_monitor.lateral_acceleration.data =
odom_history_ptr_->at(odom_size - 1).twist.twist.linear.x *
tan(current_vec_steering_msg_ptr_->steering_tire_angle) / wheelbase_;

if (odom_history_ptr_->size() >= 2) {
// Calculate longitudinal acceleration

double dv = odom_history_ptr_->at(odom_size - 1).twist.twist.linear.x -
odom_history_ptr_->at(odom_size - 2).twist.twist.linear.x;

auto duration =
(rclcpp::Time(odom_history_ptr_->at(odom_size - 1).header.stamp) -
rclcpp::Time(odom_history_ptr_->at(odom_size - 2).header.stamp));

double dt = std::max(duration.seconds(), 1e-03);

driving_status_vars.driving_monitor.longitudinal_acceleration.data = dv / dt;
driving_status_vars.driving_monitor.longitudinal_acceleration.header.set__stamp(
rclcpp::Time(odom_history_ptr_->at(odom_size - 1).header.stamp) +
duration * 0.5); // Time stamp of acceleration data

// Calculate lateral jerk

if (
driving_status_vars.driving_monitor.lateral_acceleration.header.stamp !=
prev_driving_vars_->driving_monitor.lateral_acceleration.header.stamp) {
double d_lateral_jerk = driving_status_vars.driving_monitor.lateral_acceleration.data -
prev_driving_vars_->driving_monitor.lateral_acceleration.data;

// We already know the delta time from above. same as longitudinal acceleration

driving_status_vars.driving_monitor.lateral_jerk.data = d_lateral_jerk / dt;
driving_status_vars.driving_monitor.lateral_jerk.header =
driving_status_vars.driving_monitor.longitudinal_acceleration.header;
}
}
if (odom_history_ptr_->size() == 3) {
// calculate longitudinal jerk
double d_a = driving_status_vars.driving_monitor.longitudinal_acceleration.data -
prev_driving_vars_->driving_monitor.longitudinal_acceleration.data;
auto duration =
(rclcpp::Time(
driving_status_vars.driving_monitor.longitudinal_acceleration.header.stamp) -
rclcpp::Time(
prev_driving_vars_->driving_monitor.longitudinal_acceleration.header.stamp));
double dt = std::max(duration.seconds(), 1e-03);
driving_status_vars.driving_monitor.longitudinal_jerk.data = d_a / dt;
driving_status_vars.driving_monitor.longitudinal_jerk.header.set__stamp(
rclcpp::Time(prev_driving_vars_->driving_monitor.longitudinal_acceleration.header.stamp) +
duration * 0.5); // Time stamp of jerk data
}
prev_driving_vars_ =
std::move(std::make_unique<msg::DrivingMonitorStamped>(driving_status_vars));
last_odom_header.stamp = odom_history_ptr_->at(odom_size - 1).header.stamp;


if (odom_history_ptr_->size() >= odom_interval_ + 2) {

// Calculate longitudinal acceleration

double dv = odom_history_ptr_->at(odom_size - 1).twist.twist.linear.x -
odom_history_ptr_->at(odom_size - odom_interval_ - 2).twist.twist.linear.x;

auto duration =
(rclcpp::Time(odom_history_ptr_->at(odom_size - 1).header.stamp) -
rclcpp::Time(odom_history_ptr_->at(odom_size - odom_interval_ - 2).header.stamp));

double dt = std::max(duration.seconds(), 1e-03);

driving_status_vars.driving_monitor.longitudinal_acceleration.data = dv / dt;
driving_status_vars.driving_monitor.longitudinal_acceleration.header.set__stamp(
rclcpp::Time(odom_history_ptr_->at(odom_size - odom_interval_ - 2).header.stamp) +
duration * 0.5); // Time stamp of acceleration data

// Calculate lateral jerk

double d_lateral_jerk = driving_status_vars.driving_monitor.lateral_acceleration.data -
prev_driving_vars_->driving_monitor.lateral_acceleration.data;

// We already know the delta time from above. same as longitudinal acceleration

driving_status_vars.driving_monitor.lateral_jerk.data = d_lateral_jerk / dt;
driving_status_vars.driving_monitor.lateral_jerk.header =
driving_status_vars.driving_monitor.longitudinal_acceleration.header;
}

if (odom_history_ptr_->size() == 2 * odom_interval_ + 3) {
// calculate longitudinal jerk
double d_a = driving_status_vars.driving_monitor.longitudinal_acceleration.data -
prev_driving_vars_->driving_monitor.longitudinal_acceleration.data;
auto duration =
(rclcpp::Time(
driving_status_vars.driving_monitor.longitudinal_acceleration.header.stamp) -
rclcpp::Time(
prev_driving_vars_->driving_monitor.longitudinal_acceleration.header.stamp));
double dt = std::max(duration.seconds(), 1e-03);
driving_status_vars.driving_monitor.longitudinal_jerk.data = d_a / dt;
driving_status_vars.driving_monitor.longitudinal_jerk.header.set__stamp(
rclcpp::Time(prev_driving_vars_->driving_monitor.longitudinal_acceleration.header.stamp) +
duration * 0.5); // Time stamp of jerk data
}

prev_driving_vars_ =
std::move(std::make_unique<msg::DrivingMonitorStamped>(driving_status_vars));

last_odom_header.stamp = odom_history_ptr_->at(odom_size - 1).header.stamp;
}
return true;

Expand Down Expand Up @@ -360,12 +370,12 @@ void ControlPerformanceAnalysisCore::findCurveRefIdx()
}

auto fun_distance_cond = [this](auto pose_t) {
double dist = std::hypot(
pose_t.position.x - this->interpolated_pose_ptr_->position.x,
pose_t.position.y - this->interpolated_pose_ptr_->position.y);
double dist = std::hypot(
pose_t.position.x - this->interpolated_pose_ptr_->position.x,
pose_t.position.y - this->interpolated_pose_ptr_->position.y);

return dist > wheelbase_;
};
return dist > wheelbase_;
};

auto it = std::find_if(
current_waypoints_ptr_->poses.cbegin() + *idx_prev_wp_, current_waypoints_ptr_->poses.cend(),
Expand Down Expand Up @@ -418,10 +428,10 @@ std::pair<bool, Pose> ControlPerformanceAnalysisCore::calculateClosestPose()

// Previous waypoint to next waypoint.
double dx_prev2next = current_waypoints_ptr_->poses.at(*idx_next_wp_).position.x -
current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.x;
current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.x;

double dy_prev2next = current_waypoints_ptr_->poses.at(*idx_next_wp_).position.y -
current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.y;
current_waypoints_ptr_->poses.at(*idx_prev_wp_).position.y;

double delta_psi_prev2next = utils::angleDistance(next_yaw, prev_yaw);
double d_vel_prev2next = next_velocity - prev_velocity;
Expand Down Expand Up @@ -521,9 +531,9 @@ double ControlPerformanceAnalysisCore::estimateCurvature()
std::distance(current_waypoints_ptr_->poses.cbegin(), current_waypoints_ptr_->poses.cend());

auto num_of_forward_indices = num_of_back_indices;
int32_t loc_of_forward_idx = (*idx_curve_ref_wp_ + num_of_forward_indices > max_idx)
? max_idx - 1
: *idx_curve_ref_wp_ + num_of_forward_indices - 1;
int32_t loc_of_forward_idx = (*idx_curve_ref_wp_ + num_of_forward_indices > max_idx) ?
max_idx - 1 :
*idx_curve_ref_wp_ + num_of_forward_indices - 1;

// We have three indices of the three trajectory poses.
// We compute a curvature estimate from these points.
Expand Down Expand Up @@ -559,12 +569,12 @@ double ControlPerformanceAnalysisCore::estimatePurePursuitCurvature()
double look_ahead_distance_pp = std::max(wheelbase_, 2 * Vx);

auto fun_distance_cond = [this, &look_ahead_distance_pp](auto pose_t) {
double dist = std::hypot(
pose_t.position.x - this->interpolated_pose_ptr_->position.x,
pose_t.position.y - this->interpolated_pose_ptr_->position.y);
double dist = std::hypot(
pose_t.position.x - this->interpolated_pose_ptr_->position.x,
pose_t.position.y - this->interpolated_pose_ptr_->position.y);

return dist > look_ahead_distance_pp;
};
return dist > look_ahead_distance_pp;
};

auto it = std::find_if(
current_waypoints_ptr_->poses.cbegin() + *idx_prev_wp_, current_waypoints_ptr_->poses.cend(),
Expand Down
3 changes: 2 additions & 1 deletion control/control_performance_analysis/src/control_performance_analysis_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,10 +48,11 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode(
param_.control_period = declare_parameter("control_period", 0.033);
param_.curvature_interval_length = declare_parameter("curvature_interval_length", 10.0);
param_.lpf_gain = declare_parameter("control_cmd_lpf_gain", 0.5);
param_.odom_interval = declare_parameter("odom_interval", 2);

// Prepare error computation class with the wheelbase parameter.
control_performance_core_ptr_ = std::make_unique<ControlPerformanceAnalysisCore>(
param_.wheel_base, param_.curvature_interval_length);
param_.wheel_base, param_.curvature_interval_length, param_.odom_interval);

// Subscribers.
sub_trajectory_ = create_subscription<Trajectory>(
Expand Down

0 comments on commit f569d71

Please sign in to comment.