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(trajectory_follower): use kinematic topic for ego pose #1791

Merged
merged 4 commits into from
Sep 7, 2022
Merged

feat(trajectory_follower): use kinematic topic for ego pose #1791

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
269765e
use constPtr
TakaHoribe Sep 6, 2022
c975b3d
[velocity_controller] use topic for ego-pose, not TF
TakaHoribe Sep 6, 2022
a0fda5b
[mpc] use topic for ego-pose, not TF
TakaHoribe Sep 6, 2022
a639e4b
rename prev variable
TakaHoribe Sep 7, 2022
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
3 changes: 1 addition & 2 deletions control/trajectory_follower/include/trajectory_follower/mpc.hpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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_velocity_ptr{nullptr};
autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr m_trajectory_ptr{nullptr};
takayuki5168 marked this conversation as resolved.
Show resolved Hide resolved

// 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
Open in desktop
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
Open in desktop
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
33 changes: 11 additions & 22 deletions control/trajectory_follower/src/pid_longitudinal_controller.cpp
View file
Open in desktop
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_velocity_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_velocity_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 @@ -713,15 +702,15 @@ 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;
m_current_kinematic_state_ptr->twist.twist.linear.x - m_prev_velocity_ptr->twist.twist.linear.x;
const float64_t dt = std::max(
(rclcpp::Time(m_current_velocity_ptr->header.stamp) -
(rclcpp::Time(m_current_kinematic_state_ptr->header.stamp) -
rclcpp::Time(m_prev_velocity_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
Open in desktop
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