Skip to content

Commit

Permalink
feat(trajectory_follower): use kinematic topic for ego pose (autoware…
Browse files Browse the repository at this point in the history 
…foundation#1791)

* use constPtr

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* [velocity_controller] use topic for ego-pose, not TF

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* [mpc] use topic for ego-pose, not TF

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* rename prev variable

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
  • Loading branch information
@TakaHoribe @boyali
TakaHoribe authored and boyali committed Oct 3, 2022
1 parent 8e1cc06 commit 07d014f
Show file tree
Hide file tree
Showing 7 changed files with 42 additions and 105 deletions.
3 changes: 1 addition & 2 deletions control/trajectory_follower/include/trajectory_follower/mpc.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -395,8 +395,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg,
const float64_t traj_resample_dist, const bool8_t enable_path_smoothing,
const int64_t path_filter_moving_ave_num, const int64_t curvature_smoothing_num_traj,
const int64_t curvature_smoothing_num_ref_steer,
const geometry_msgs::msg::PoseStamped::SharedPtr current_pose_ptr);
const int64_t curvature_smoothing_num_ref_steer);
/**
* @brief set the vehicle model of this MPC
*/
Expand Down
16 changes: 2 additions & 14 deletions control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,10 +114,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
// MPC object
trajectory_follower::MPC m_mpc;

//!< @brief measured pose
geometry_msgs::msg::PoseStamped::SharedPtr m_current_pose_ptr;
//!< @brief measured velocity
nav_msgs::msg::Odometry::SharedPtr m_current_odometry_ptr;
//!< @brief measured kinematic state
nav_msgs::msg::Odometry::SharedPtr m_current_kinematic_state_ptr;
//!< @brief measured steering
autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr m_current_steering_ptr;
//!< @brief reference trajectory
Expand All @@ -134,10 +132,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
//!< @brief flag whether the first trajectory has been received
bool m_has_received_first_trajectory = false;

//!< @brief buffer for transforms
tf2::BufferCore m_tf_buffer{tf2::BUFFER_CORE_DEFAULT_CACHE_TIME};
tf2_ros::TransformListener m_tf_listener{m_tf_buffer};

// ego nearest index search
double m_ego_nearest_dist_threshold;
double m_ego_nearest_yaw_threshold;
Expand All @@ -159,12 +153,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
*/
void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr);

/**
* @brief update current_pose from tf
* @return true if the current pose was updated, false otherwise
*/
bool8_t updateCurrentPose();

/**
* @brief check if the received data is valid.
*/
Expand Down
13 changes: 4 additions & 9 deletions control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,19 +91,14 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
rclcpp::Publisher<autoware_auto_system_msgs::msg::Float32MultiArrayDiagnostic>::SharedPtr
m_pub_debug;

rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr m_tf_sub;
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr m_tf_static_sub;
tf2::BufferCore m_tf_buffer{tf2::BUFFER_CORE_DEFAULT_CACHE_TIME};
tf2_ros::TransformListener m_tf_listener{m_tf_buffer};

rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res;
rcl_interfaces::msg::SetParametersResult paramCallback(
const std::vector<rclcpp::Parameter> & parameters);

// pointers for ros topic
std::shared_ptr<nav_msgs::msg::Odometry> m_current_velocity_ptr{nullptr};
std::shared_ptr<nav_msgs::msg::Odometry> m_prev_velocity_ptr{nullptr};
std::shared_ptr<autoware_auto_planning_msgs::msg::Trajectory> m_trajectory_ptr{nullptr};
nav_msgs::msg::Odometry::ConstSharedPtr m_current_kinematic_state_ptr{nullptr};
nav_msgs::msg::Odometry::ConstSharedPtr m_prev_kienmatic_state_ptr{nullptr};
autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr m_trajectory_ptr{nullptr};

// vehicle info
float64_t m_wheel_base;
Expand Down Expand Up @@ -216,7 +211,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
* @brief set current and previous velocity with received message
* @param [in] msg current state message
*/
void setCurrentVelocity(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
void setKinematicState(const nav_msgs::msg::Odometry::ConstSharedPtr msg);

/**
* @brief set reference trajectory with received message
Expand Down
9 changes: 3 additions & 6 deletions control/trajectory_follower/src/mpc.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -181,8 +181,7 @@ void MPC::setReferenceTrajectory(
const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg,
const float64_t traj_resample_dist, const bool8_t enable_path_smoothing,
const int64_t path_filter_moving_ave_num, const int64_t curvature_smoothing_num_traj,
const int64_t curvature_smoothing_num_ref_steer,
const geometry_msgs::msg::PoseStamped::SharedPtr current_pose_ptr)
const int64_t curvature_smoothing_num_ref_steer)
{
trajectory_follower::MPCTrajectory mpc_traj_raw; // received raw trajectory
trajectory_follower::MPCTrajectory mpc_traj_resampled; // resampled trajectory
Expand Down Expand Up @@ -220,10 +219,8 @@ void MPC::setReferenceTrajectory(
}

/* calculate yaw angle */
if (current_pose_ptr) {
trajectory_follower::MPCUtils::calcTrajectoryYawFromXY(&mpc_traj_smoothed, m_is_forward_shift);
trajectory_follower::MPCUtils::convertEulerAngleToMonotonic(&mpc_traj_smoothed.yaw);
}
trajectory_follower::MPCUtils::calcTrajectoryYawFromXY(&mpc_traj_smoothed, m_is_forward_shift);
trajectory_follower::MPCUtils::convertEulerAngleToMonotonic(&mpc_traj_smoothed.yaw);

/* calculate curvature */
trajectory_follower::MPCUtils::calcTrajectoryCurvature(
Expand Down
51 changes: 13 additions & 38 deletions control/trajectory_follower/src/mpc_lateral_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -177,7 +177,7 @@ MpcLateralController::~MpcLateralController() {}

boost::optional<LateralOutput> MpcLateralController::run()
{
if (!checkData() || !updateCurrentPose()) {
if (!checkData()) {
return boost::none;
}

Expand All @@ -191,8 +191,8 @@ boost::optional<LateralOutput> MpcLateralController::run()
}

const bool8_t is_mpc_solved = m_mpc.calculateMPC(
*m_current_steering_ptr, m_current_odometry_ptr->twist.twist.linear.x, m_current_pose_ptr->pose,
ctrl_cmd, predicted_traj, diagnostic);
*m_current_steering_ptr, m_current_kinematic_state_ptr->twist.twist.linear.x,
m_current_kinematic_state_ptr->pose.pose, ctrl_cmd, predicted_traj, diagnostic);

publishPredictedTraj(predicted_traj);
publishDiagnostic(diagnostic);
Expand Down Expand Up @@ -228,7 +228,7 @@ boost::optional<LateralOutput> MpcLateralController::run()
void MpcLateralController::setInputData(InputData const & input_data)
{
setTrajectory(input_data.current_trajectory_ptr);
m_current_odometry_ptr = input_data.current_odometry_ptr;
m_current_kinematic_state_ptr = input_data.current_odometry_ptr;
m_current_steering_ptr = input_data.current_steering_ptr;
}

Expand Down Expand Up @@ -259,10 +259,10 @@ bool8_t MpcLateralController::checkData() const
return false;
}

if (!m_current_odometry_ptr) {
if (!m_current_kinematic_state_ptr) {
RCLCPP_DEBUG(
node_->get_logger(), "waiting data. current_velocity = %d",
m_current_odometry_ptr != nullptr);
node_->get_logger(), "waiting data. kinematic_state = %d",
m_current_kinematic_state_ptr != nullptr);
return false;
}

Expand All @@ -288,8 +288,8 @@ void MpcLateralController::setTrajectory(

m_current_trajectory_ptr = msg;

if (!m_current_pose_ptr && !updateCurrentPose()) {
RCLCPP_DEBUG(node_->get_logger(), "Current pose is not received yet.");
if (!m_current_kinematic_state_ptr) {
RCLCPP_DEBUG(node_->get_logger(), "Current kinematic state is not received yet.");
return;
}

Expand All @@ -305,7 +305,7 @@ void MpcLateralController::setTrajectory(

m_mpc.setReferenceTrajectory(
*msg, m_traj_resample_dist, m_enable_path_smoothing, m_path_filter_moving_ave_num,
m_curvature_smoothing_num_traj, m_curvature_smoothing_num_ref_steer, m_current_pose_ptr);
m_curvature_smoothing_num_traj, m_curvature_smoothing_num_ref_steer);

// update trajectory buffer to check the trajectory shape change.
m_trajectory_buffer.push_back(*m_current_trajectory_ptr);
Expand All @@ -324,31 +324,6 @@ void MpcLateralController::setTrajectory(
}
}

bool8_t MpcLateralController::updateCurrentPose()
{
geometry_msgs::msg::TransformStamped transform;
try {
transform = m_tf_buffer.lookupTransform(
m_current_trajectory_ptr->header.frame_id, "base_link", tf2::TimePointZero);
} catch (tf2::TransformException & ex) {
RCLCPP_WARN_SKIPFIRST_THROTTLE(
node_->get_logger(), *node_->get_clock(), 5000 /*ms*/, ex.what());
RCLCPP_WARN_SKIPFIRST_THROTTLE(
node_->get_logger(), *node_->get_clock(), 5000 /*ms*/,
m_tf_buffer.allFramesAsString().c_str());
return false;
}

geometry_msgs::msg::PoseStamped ps;
ps.header = transform.header;
ps.pose.position.x = transform.transform.translation.x;
ps.pose.position.y = transform.transform.translation.y;
ps.pose.position.z = transform.transform.translation.z;
ps.pose.orientation = transform.transform.rotation;
m_current_pose_ptr = std::make_shared<geometry_msgs::msg::PoseStamped>(ps);
return true;
}

autoware_auto_control_msgs::msg::AckermannLateralCommand
MpcLateralController::getStopControlCommand() const
{
Expand Down Expand Up @@ -379,10 +354,10 @@ bool8_t MpcLateralController::isStoppedState() const
// control was turned off when approaching/exceeding the stop line on a curve or
// emergency stop situation and it caused large tracking error.
const size_t nearest = motion_utils::findFirstNearestIndexWithSoftConstraints(
m_current_trajectory_ptr->points, m_current_pose_ptr->pose, m_ego_nearest_dist_threshold,
m_ego_nearest_yaw_threshold);
m_current_trajectory_ptr->points, m_current_kinematic_state_ptr->pose.pose,
m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold);

const float64_t current_vel = m_current_odometry_ptr->twist.twist.linear.x;
const float64_t current_vel = m_current_kinematic_state_ptr->twist.twist.linear.x;
const float64_t target_vel =
m_current_trajectory_ptr->points.at(static_cast<size_t>(nearest)).longitudinal_velocity_mps;

Expand Down
37 changes: 13 additions & 24 deletions control/trajectory_follower/src/pid_longitudinal_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -206,18 +206,17 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node
void PidLongitudinalController::setInputData(InputData const & input_data)
{
setTrajectory(input_data.current_trajectory_ptr);
setCurrentVelocity(input_data.current_odometry_ptr);
setKinematicState(input_data.current_odometry_ptr);
}

void PidLongitudinalController::setCurrentVelocity(
const nav_msgs::msg::Odometry::ConstSharedPtr msg)
void PidLongitudinalController::setKinematicState(const nav_msgs::msg::Odometry::ConstSharedPtr msg)
{
if (!msg) return;

if (m_current_velocity_ptr) {
m_prev_velocity_ptr = m_current_velocity_ptr;
if (m_current_kinematic_state_ptr) {
m_prev_kienmatic_state_ptr = m_current_kinematic_state_ptr;
}
m_current_velocity_ptr = std::make_shared<nav_msgs::msg::Odometry>(*msg);
m_current_kinematic_state_ptr = msg;
}

void PidLongitudinalController::setTrajectory(
Expand All @@ -237,7 +236,7 @@ void PidLongitudinalController::setTrajectory(
return;
}

m_trajectory_ptr = std::make_shared<autoware_auto_planning_msgs::msg::Trajectory>(*msg);
m_trajectory_ptr = msg;
}

rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallback(
Expand Down Expand Up @@ -366,22 +365,12 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac
boost::optional<LongitudinalOutput> PidLongitudinalController::run()
{
// wait for initial pointers
if (
!m_current_velocity_ptr || !m_prev_velocity_ptr || !m_trajectory_ptr ||
!m_tf_buffer.canTransform(m_trajectory_ptr->header.frame_id, "base_link", tf2::TimePointZero)) {
if (!m_current_kinematic_state_ptr || !m_prev_kienmatic_state_ptr || !m_trajectory_ptr) {
return boost::none;
}

// get current ego pose
geometry_msgs::msg::TransformStamped tf =
m_tf_buffer.lookupTransform(m_trajectory_ptr->header.frame_id, "base_link", tf2::TimePointZero);

// calculate current pose and control data
geometry_msgs::msg::Pose current_pose;
current_pose.position.x = tf.transform.translation.x;
current_pose.position.y = tf.transform.translation.y;
current_pose.position.z = tf.transform.translation.z;
current_pose.orientation = tf.transform.rotation;
geometry_msgs::msg::Pose current_pose = m_current_kinematic_state_ptr->pose.pose;

const auto control_data = getControlData(current_pose);

Expand Down Expand Up @@ -712,16 +701,16 @@ float64_t PidLongitudinalController::getDt()

PidLongitudinalController::Motion PidLongitudinalController::getCurrentMotion() const
{
const float64_t dv =
m_current_velocity_ptr->twist.twist.linear.x - m_prev_velocity_ptr->twist.twist.linear.x;
const float64_t dv = m_current_kinematic_state_ptr->twist.twist.linear.x -
m_prev_kienmatic_state_ptr->twist.twist.linear.x;
const float64_t dt = std::max(
(rclcpp::Time(m_current_velocity_ptr->header.stamp) -
rclcpp::Time(m_prev_velocity_ptr->header.stamp))
(rclcpp::Time(m_current_kinematic_state_ptr->header.stamp) -
rclcpp::Time(m_prev_kienmatic_state_ptr->header.stamp))
.seconds(),
1e-03);
const float64_t accel = dv / dt;

const float64_t current_vel = m_current_velocity_ptr->twist.twist.linear.x;
const float64_t current_vel = m_current_kinematic_state_ptr->twist.twist.linear.x;
const float64_t current_acc = m_lpf_acc->filter(accel);

return Motion{current_vel, current_acc};
Expand Down
18 changes: 6 additions & 12 deletions control/trajectory_follower/test/test_mpc.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -177,8 +177,7 @@ class MPCTest : public ::testing::Test
// Init trajectory
mpc.setReferenceTrajectory(
dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing,
path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer,
pose_zero_ptr);
path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer);
}
}; // class MPCTest

Expand Down Expand Up @@ -236,8 +235,7 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn)
initializeMPC(mpc);
mpc.setReferenceTrajectory(
dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing,
path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer,
pose_zero_ptr);
path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer);

// Calculate MPC
AckermannLateralCommand ctrl_cmd;
Expand All @@ -255,8 +253,7 @@ TEST_F(MPCTest, OsqpCalculate)
initializeMPC(mpc);
mpc.setReferenceTrajectory(
dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing,
path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer,
pose_zero_ptr);
path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer);

const std::string vehicle_model_type = "kinematics";
std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr =
Expand Down Expand Up @@ -287,8 +284,7 @@ TEST_F(MPCTest, OsqpCalculateRightTurn)
initializeMPC(mpc);
mpc.setReferenceTrajectory(
dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing,
path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer,
pose_zero_ptr);
path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer);

const std::string vehicle_model_type = "kinematics";
std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr =
Expand Down Expand Up @@ -333,8 +329,7 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate)
// Init trajectory
mpc.setReferenceTrajectory(
dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing,
path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer,
pose_zero_ptr);
path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer);
// Calculate MPC
AckermannLateralCommand ctrl_cmd;
Trajectory pred_traj;
Expand All @@ -351,8 +346,7 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn)
initializeMPC(mpc);
mpc.setReferenceTrajectory(
dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing,
path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer,
pose_zero_ptr);
path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer);

const std::string vehicle_model_type = "kinematics_no_delay";
std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr =
Expand Down

0 comments on commit 07d014f

Please sign in to comment.