Skip to content
This repository has been archived by the owner on Sep 16, 2022. It is now read-only.

Sync trajectory_follower_nodes #715

Merged
merged 1 commit into from
Nov 19, 2021
Merged

Sync trajectory_follower_nodes #715

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
1 change: 0 additions & 1 deletion ...l/trajectory_follower_nodes/include/trajectory_follower_nodes/lateral_controller_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -112,7 +112,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC LateralController : public rclcpp::Node
/* parameters for stop state */
float64_t m_stop_state_entry_ego_speed;
float64_t m_stop_state_entry_target_speed;
float64_t m_stop_state_keep_stopping_dist;

// MPC object
trajectory_follower::MPC m_mpc;
Expand Down
1 change: 0 additions & 1 deletion control/trajectory_follower_nodes/param/lateral_controller_defaults.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,6 @@
# stop state
stop_state_entry_ego_speed: 0.2
stop_state_entry_target_speed: 0.5
stop_state_keep_stopping_dist: 0.5

# vehicle parameters
mass_kg: 2400.0
Expand Down
18 changes: 5 additions & 13 deletions control/trajectory_follower_nodes/src/lateral_controller_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,8 +71,6 @@ LateralController::LateralController(const rclcpp::NodeOptions & node_options)
m_stop_state_entry_ego_speed = declare_parameter<float64_t>("stop_state_entry_ego_speed");
m_stop_state_entry_target_speed =
declare_parameter<float64_t>("stop_state_entry_target_speed");
m_stop_state_keep_stopping_dist =
declare_parameter<float64_t>("stop_state_keep_stopping_dist");

/* mpc parameters */
const float64_t steer_lim_deg = declare_parameter<float64_t>("steer_lim_deg");
Expand Down Expand Up @@ -345,22 +343,16 @@ const

bool8_t LateralController::isStoppedState() const
{
// Note: This function used to take into account the distance to the stop line
// for the stop state judgement. However, it has been removed since the steering
// control was turned off when approaching/exceeding the stop line on a curve or
// emergency stop situation and it caused large tracking error.
const int64_t nearest = trajectory_follower::MPCUtils::calcNearestIndex(
*m_current_trajectory_ptr,
m_current_pose_ptr->pose);

// If the nearest index is not found, return false
if (nearest < 0) {return false;}
const float64_t dist = trajectory_follower::MPCUtils::calcStopDistance(
*m_current_trajectory_ptr,
nearest);
if (dist < m_stop_state_keep_stopping_dist) {
RCLCPP_DEBUG(
get_logger(),
"stop_dist = %f < %f : m_stop_state_keep_stopping_dist. keep stopping.", dist,
m_stop_state_keep_stopping_dist);
return true;
}
RCLCPP_DEBUG(get_logger(), "stop_dist = %f release stopping.", dist);

const float64_t current_vel = m_current_odometry_ptr->twist.twist.linear.x;
const float64_t target_vel =
Expand Down