Skip to content

Commit

Permalink
refactor(trajectory_follower/trajectory_follower_nodes): remove autow…
Browse files Browse the repository at this point in the history 
…are_auto_common dependency (autowarefoundation#2228)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
  • Loading branch information
@takayuki5168
takayuki5168 committed Nov 9, 2022
1 parent 0d9c92f commit 536a958
Show file tree
Hide file tree
Showing 41 changed files with 985 additions and 1,054 deletions.
11 changes: 5 additions & 6 deletions control/trajectory_follower/include/trajectory_follower/debug_values.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@
#ifndef TRAJECTORY_FOLLOWER__DEBUG_VALUES_HPP_
#define TRAJECTORY_FOLLOWER__DEBUG_VALUES_HPP_

#include "common/types.hpp"
#include "trajectory_follower/visibility_control.hpp"

#include <array>
Expand All @@ -28,7 +27,7 @@ namespace control
{
namespace trajectory_follower
{
using autoware::common::types::float64_t;

/// Debug Values used for debugging or controller tuning
class TRAJECTORY_FOLLOWER_PUBLIC DebugValues
{
Expand Down Expand Up @@ -77,13 +76,13 @@ class TRAJECTORY_FOLLOWER_PUBLIC DebugValues
* @brief get all the debug values as an std::array
* @return array of all debug values
*/
std::array<float64_t, static_cast<size_t>(TYPE::SIZE)> getValues() const { return m_values; }
std::array<double, static_cast<size_t>(TYPE::SIZE)> getValues() const { return m_values; }
/**
* @brief set the given type to the given value
* @param [in] type TYPE of the value
* @param [in] value value to set
*/
void setValues(const TYPE type, const float64_t value)
void setValues(const TYPE type, const double value)
{
m_values.at(static_cast<size_t>(type)) = value;
}
Expand All @@ -92,10 +91,10 @@ class TRAJECTORY_FOLLOWER_PUBLIC DebugValues
* @param [in] type index of the type
* @param [in] value value to set
*/
void setValues(const size_t type, const float64_t value) { m_values.at(type) = value; }
void setValues(const size_t type, const double value) { m_values.at(type) = value; }

private:
std::array<float64_t, static_cast<size_t>(TYPE::SIZE)> m_values;
std::array<double, static_cast<size_t>(TYPE::SIZE)> m_values;
};
} // namespace trajectory_follower
} // namespace control
Expand Down
16 changes: 7 additions & 9 deletions control/trajectory_follower/include/trajectory_follower/interpolate.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@
#ifndef TRAJECTORY_FOLLOWER__INTERPOLATE_HPP_
#define TRAJECTORY_FOLLOWER__INTERPOLATE_HPP_

#include "common/types.hpp"
#include "trajectory_follower/visibility_control.hpp"

#include <cmath>
Expand All @@ -30,28 +29,27 @@ namespace control
{
namespace trajectory_follower
{
using autoware::common::types::bool8_t;
using autoware::common::types::float64_t;

/**
* @brief linearly interpolate the given values assuming a base indexing and a new desired indexing
* @param [in] base_index indexes for each base value
* @param [in] base_value values for each base index
* @param [in] return_index desired interpolated indexes
* @param [out] return_value resulting interpolated values
*/
TRAJECTORY_FOLLOWER_PUBLIC bool8_t linearInterpolate(
const std::vector<float64_t> & base_index, const std::vector<float64_t> & base_value,
const std::vector<float64_t> & return_index, std::vector<float64_t> & return_value);
TRAJECTORY_FOLLOWER_PUBLIC bool linearInterpolate(
const std::vector<double> & base_index, const std::vector<double> & base_value,
const std::vector<double> & return_index, std::vector<double> & return_value);
/**
* @brief linearly interpolate the given values assuming a base indexing and a new desired index
* @param [in] base_index indexes for each base value
* @param [in] base_value values for each base index
* @param [in] return_index desired interpolated index
* @param [out] return_value resulting interpolated value
*/
TRAJECTORY_FOLLOWER_PUBLIC bool8_t linearInterpolate(
const std::vector<float64_t> & base_index, const std::vector<float64_t> & base_value,
const float64_t & return_index, float64_t & return_value);
TRAJECTORY_FOLLOWER_PUBLIC bool linearInterpolate(
const std::vector<double> & base_index, const std::vector<double> & base_value,
const double & return_index, double & return_value);
} // namespace trajectory_follower
} // namespace control
} // namespace motion
Expand Down
43 changes: 20 additions & 23 deletions control/trajectory_follower/include/trajectory_follower/longitudinal_controller_utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@
#ifndef TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_UTILS_HPP_
#define TRAJECTORY_FOLLOWER__LONGITUDINAL_CONTROLLER_UTILS_HPP_

#include "common/types.hpp"
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/Geometry"
#include "geometry/common_2d.hpp"
Expand All @@ -42,8 +41,7 @@ namespace trajectory_follower
{
namespace longitudinal_utils
{
using autoware::common::types::bool8_t;
using autoware::common::types::float64_t;

using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_planning_msgs::msg::TrajectoryPoint;
using geometry_msgs::msg::Point;
Expand All @@ -53,19 +51,18 @@ using geometry_msgs::msg::Quaternion;
/**
* @brief check if trajectory is invalid or not
*/
TRAJECTORY_FOLLOWER_PUBLIC bool8_t isValidTrajectory(const Trajectory & traj);
TRAJECTORY_FOLLOWER_PUBLIC bool isValidTrajectory(const Trajectory & traj);

/**
* @brief calculate distance to stopline from current vehicle position where velocity is 0
*/
TRAJECTORY_FOLLOWER_PUBLIC float64_t calcStopDistance(
const Pose & current_pose, const Trajectory & traj, const float64_t max_dist,
const float64_t max_yaw);
TRAJECTORY_FOLLOWER_PUBLIC double calcStopDistance(
const Pose & current_pose, const Trajectory & traj, const double max_dist, const double max_yaw);

/**
* @brief calculate pitch angle from estimated current pose
*/
TRAJECTORY_FOLLOWER_PUBLIC float64_t getPitchByPose(const Quaternion & quaternion);
TRAJECTORY_FOLLOWER_PUBLIC double getPitchByPose(const Quaternion & quaternion);

/**
* @brief calculate pitch angle from trajectory on map
Expand All @@ -74,21 +71,21 @@ TRAJECTORY_FOLLOWER_PUBLIC float64_t getPitchByPose(const Quaternion & quaternio
* @param [in] closest_idx nearest index to current vehicle position
* @param [in] wheel_base length of wheel base
*/
TRAJECTORY_FOLLOWER_PUBLIC float64_t
getPitchByTraj(const Trajectory & trajectory, const size_t closest_idx, const float64_t wheel_base);
TRAJECTORY_FOLLOWER_PUBLIC double getPitchByTraj(
const Trajectory & trajectory, const size_t closest_idx, const double wheel_base);

/**
* @brief calculate elevation angle
*/
TRAJECTORY_FOLLOWER_PUBLIC float64_t
calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to);
TRAJECTORY_FOLLOWER_PUBLIC double calcElevationAngle(
const TrajectoryPoint & p_from, const TrajectoryPoint & p_to);

/**
* @brief calculate vehicle pose after time delay by moving the vehicle at current velocity for
* delayed time
*/
TRAJECTORY_FOLLOWER_PUBLIC Pose calcPoseAfterTimeDelay(
const Pose & current_pose, const float64_t delay_time, const float64_t current_vel);
const Pose & current_pose, const double delay_time, const double current_vel);

/**
* @brief apply linear interpolation to orientation
Expand All @@ -97,7 +94,7 @@ TRAJECTORY_FOLLOWER_PUBLIC Pose calcPoseAfterTimeDelay(
* @param [in] ratio ratio between o_from and o_to for interpolation
*/
TRAJECTORY_FOLLOWER_PUBLIC Quaternion
lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const float64_t ratio);
lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio);

/**
* @brief apply linear interpolation to trajectory point that is nearest to a certain point
Expand All @@ -106,16 +103,16 @@ lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const float6
*/
template <class T>
TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint lerpTrajectoryPoint(
const T & points, const Pose & pose, const float64_t max_dist, const float64_t max_yaw)
const T & points, const Pose & pose, const double max_dist, const double max_yaw)
{
TrajectoryPoint interpolated_point;
const size_t seg_idx =
motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(points, pose, max_dist, max_yaw);

const float64_t len_to_interpolated =
const double len_to_interpolated =
motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, pose.position);
const float64_t len_segment = motion_utils::calcSignedArcLength(points, seg_idx, seg_idx + 1);
const float64_t interpolate_ratio = std::clamp(len_to_interpolated / len_segment, 0.0, 1.0);
const double len_segment = motion_utils::calcSignedArcLength(points, seg_idx, seg_idx + 1);
const double interpolate_ratio = std::clamp(len_to_interpolated / len_segment, 0.0, 1.0);

{
const size_t i = seg_idx;
Expand Down Expand Up @@ -147,8 +144,8 @@ TRAJECTORY_FOLLOWER_PUBLIC TrajectoryPoint lerpTrajectoryPoint(
* @param [in] dt time between current and previous one
* @param [in] lim_val limitation value for differential
*/
TRAJECTORY_FOLLOWER_PUBLIC float64_t applyDiffLimitFilter(
const float64_t input_val, const float64_t prev_val, const float64_t dt, const float64_t lim_val);
TRAJECTORY_FOLLOWER_PUBLIC double applyDiffLimitFilter(
const double input_val, const double prev_val, const double dt, const double lim_val);

/**
* @brief limit variable whose differential is within a certain value
Expand All @@ -158,9 +155,9 @@ TRAJECTORY_FOLLOWER_PUBLIC float64_t applyDiffLimitFilter(
* @param [in] max_val maximum value for differential
* @param [in] min_val minimum value for differential
*/
TRAJECTORY_FOLLOWER_PUBLIC float64_t applyDiffLimitFilter(
const float64_t input_val, const float64_t prev_val, const float64_t dt, const float64_t max_val,
const float64_t min_val);
TRAJECTORY_FOLLOWER_PUBLIC double applyDiffLimitFilter(
const double input_val, const double prev_val, const double dt, const double max_val,
const double min_val);
} // namespace longitudinal_utils
} // namespace trajectory_follower
} // namespace control
Expand Down
53 changes: 25 additions & 28 deletions control/trajectory_follower/include/trajectory_follower/lowpass_filter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@
#ifndef TRAJECTORY_FOLLOWER__LOWPASS_FILTER_HPP_
#define TRAJECTORY_FOLLOWER__LOWPASS_FILTER_HPP_

#include "common/types.hpp"
#include "trajectory_follower/visibility_control.hpp"

#include <algorithm>
Expand All @@ -31,44 +30,42 @@ namespace control
{
namespace trajectory_follower
{
using autoware::common::types::bool8_t;
using autoware::common::types::float64_t;

/**
* @brief Simple filter with gain on the first derivative of the value
*/
class LowpassFilter1d
{
private:
float64_t m_x; //!< @brief current filtered value
float64_t m_gain; //!< @brief gain value of first-order low-pass filter
double m_x; //!< @brief current filtered value
double m_gain; //!< @brief gain value of first-order low-pass filter

public:
/**
* @brief constructor with initial value and first-order gain
* @param [in] x initial value
* @param [in] gain first-order gain
*/
LowpassFilter1d(const float64_t x, const float64_t gain) : m_x(x), m_gain(gain) {}
LowpassFilter1d(const double x, const double gain) : m_x(x), m_gain(gain) {}

/**
* @brief set the current value of the filter
* @param [in] x new value
*/
void reset(const float64_t x) { m_x = x; }
void reset(const double x) { m_x = x; }

/**
* @brief get the current value of the filter
*/
float64_t getValue() const { return m_x; }
double getValue() const { return m_x; }

/**
* @brief filter a new value
* @param [in] u new value
*/
float64_t filter(const float64_t u)
double filter(const double u)
{
const float64_t ret = m_gain * m_x + (1.0 - m_gain) * u;
const double ret = m_gain * m_x + (1.0 - m_gain) * u;
m_x = ret;
return ret;
}
Expand All @@ -81,24 +78,24 @@ class LowpassFilter1d
class TRAJECTORY_FOLLOWER_PUBLIC Butterworth2dFilter
{
private:
float64_t m_y1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
float64_t m_y2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
float64_t m_u1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
float64_t m_u2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
float64_t m_a0; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
float64_t m_a1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
float64_t m_a2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
float64_t m_b0; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
float64_t m_b1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
float64_t m_b2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
double m_y1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
double m_y2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
double m_u1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
double m_u2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
double m_a0; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
double m_a1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
double m_a2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
double m_b0; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
double m_b1; //!< @brief filter coefficient calculated with cutoff frequency and sampling time
double m_b2; //!< @brief filter coefficient calculated with cutoff frequency and sampling time

public:
/**
* @brief constructor with initialization
* @param [in] dt sampling time
* @param [in] f_cutoff_hz cutoff frequency [Hz]
*/
explicit Butterworth2dFilter(float64_t dt = 0.01, float64_t f_cutoff_hz = 5.0);
explicit Butterworth2dFilter(double dt = 0.01, double f_cutoff_hz = 5.0);

/**
* @brief destructor
Expand All @@ -110,36 +107,36 @@ class TRAJECTORY_FOLLOWER_PUBLIC Butterworth2dFilter
* @param [in] dt sampling time
* @param [in] f_cutoff_hz cutoff frequency [Hz]
*/
void initialize(const float64_t & dt, const float64_t & f_cutoff_hz);
void initialize(const double & dt, const double & f_cutoff_hz);

/**
* @brief filtering (call this function at each sampling time with input)
* @param [in] u scalar input for filter
* @return filtered scalar value
*/
float64_t filter(const float64_t & u);
double filter(const double & u);

/**
* @brief filtering for time-series data
* @param [in] t time-series data for input vector
* @param [out] u object vector
*/
void filt_vector(const std::vector<float64_t> & t, std::vector<float64_t> & u) const;
void filt_vector(const std::vector<double> & t, std::vector<double> & u) const;

/**
* @brief filtering for time-series data from both forward-backward direction for zero phase delay
* @param [in] t time-series data for input vector
* @param [out] u object vector
*/
void filtfilt_vector(
const std::vector<float64_t> & t,
std::vector<float64_t> & u) const; // filtering forward and backward direction
const std::vector<double> & t,
std::vector<double> & u) const; // filtering forward and backward direction

/**
* @brief get filter coefficients
* @param [out] coeffs coefficients of filter [a0, a1, a2, b0, b1, b2].
*/
void getCoefficients(std::vector<float64_t> & coeffs) const;
void getCoefficients(std::vector<double> & coeffs) const;
};

/**
Expand All @@ -152,7 +149,7 @@ namespace MoveAverageFilter
* @param [in] num index distance for moving average filter
* @param [out] u object vector
*/
TRAJECTORY_FOLLOWER_PUBLIC bool8_t filt_vector(const int64_t num, std::vector<float64_t> & u);
TRAJECTORY_FOLLOWER_PUBLIC bool filt_vector(const int num, std::vector<double> & u);
} // namespace MoveAverageFilter
} // namespace trajectory_follower
} // namespace control
Expand Down
Loading

0 comments on commit 536a958

Please sign in to comment.