Skip to content

Commit 80b9f4c

Browse files
committed
Signed-off-by: Berkay Karaman <brkay54@gmail.com>
1 parent 32c45f9 commit 80b9f4c

File tree

1 file changed

+5
-5
lines changed

1 file changed

+5
-5
lines changed

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

+5-5
Original file line numberDiff line numberDiff line change
@@ -463,8 +463,9 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData
463463
target_interpolated_point.first);
464464
}
465465

466-
std::cout << control_data.state_after_delay.running_distance << " " << control_data.nearest_idx
467-
<< " " << control_data.target_idx << std::endl;
466+
// std::cout << control_data.state_after_delay.running_distance << " " <<
467+
// control_data.nearest_idx
468+
// << " " << control_data.target_idx << std::endl;
468469
// calculate the predicted velocity in the target point
469470
m_debug_values.setValues(DebugValues::TYPE::PREDICTED_VEL, control_data.state_after_delay.vel);
470471
m_debug_values.setValues(
@@ -537,9 +538,8 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
537538
m_last_running_time = std::make_shared<rclcpp::Time>(node_->now());
538539
}
539540
const bool stopped_condition =
540-
m_last_running_time
541-
? (node_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time
542-
: false;
541+
m_last_running_time &&
542+
(node_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time;
543543

544544
static constexpr double vel_epsilon =
545545
1e-3; // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity

0 commit comments

Comments
 (0)