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(tier4_autoware_utils): add vehicle state checker #896

Merged
merged 9 commits into from
May 19, 2022
Merged

feat(tier4_autoware_utils): add vehicle state checker #896

Changes from 1 commit
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
9a42092
feat(tier4_autoware_utils): add vehicle state checker
satoshi-ota May 12, 2022
264a402
fix(tier4_autoware_utils): use absolute value
satoshi-ota May 12, 2022
57e89e3
feat(tier4_autoware_utils): divide into two classies
satoshi-ota May 15, 2022
904ada0
test(tier4_autoware_utils): add unit test for vehicle_state checker
satoshi-ota May 17, 2022
642674b
fix(tier4_autoware_utils): impl class inheritance
satoshi-ota May 17, 2022
2416f7e
docs(tier4_autoware_utils): add vehicle_state_checker document
satoshi-ota May 18, 2022
f09a0f8
fix(tier4_autoware_utils): into same loop
satoshi-ota May 19, 2022
69e5364
fix(tier4_autoware_utils): fix variables name
satoshi-ota May 19, 2022
9a85dcf
fix(tier4_autoware_utils): remove redundant codes
satoshi-ota May 19, 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
24 changes: 12 additions & 12 deletions common/tier4_autoware_utils/src/vehicle/vehicle_state_checker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,31 +36,31 @@ bool VehicleStopChecker::isVehicleStopped(const double stop_duration = 0.0) cons
return false;
}

constexpr double stop_velocity = 1e-3;
const auto now = clock_->now();
const auto time_buffer = now - twist_buffer_.back().header.stamp;
if (time_buffer.seconds() < stop_duration) {

const auto time_buffer_front = now - twist_buffer_.back().header.stamp;
isamu-takagi marked this conversation as resolved.
Show resolved Hide resolved
if (time_buffer_front.seconds() < stop_duration) {
return false;
isamu-takagi marked this conversation as resolved.
Show resolved Hide resolved
}

const auto time_buffer_back = now - twist_buffer_.front().header.stamp;
isamu-takagi marked this conversation as resolved.
Show resolved Hide resolved
if (time_buffer_back.seconds() > stop_duration) {
return twist_buffer_.front().twist.linear.x < stop_velocity;
}

// Get velocities within stop_duration
std::vector<double> vs;
for (const auto & velocity : twist_buffer_) {
vs.push_back(velocity.twist.linear.x);
if (stop_velocity <= velocity.twist.linear.x) {
return false;
}

const auto time_diff = now - velocity.header.stamp;
if (time_diff.seconds() >= stop_duration) {
break;
}
}

// Check all velocities
constexpr double stop_velocity = 1e-3;
for (const auto & v : vs) {
if (v >= stop_velocity) {
return false;
}
}

return true;
}

Expand Down