Skip to content

Commit a216d8f

Browse files
satoshi-otasatoshi-ota
committed
feat(tier4_autoware_utils): divide into two classies
1 parent 17ea748 commit a216d8f

File tree

5 files changed

+154
-78
lines changed

5 files changed

+154
-78
lines changed

‎common/tier4_autoware_utils/CMakeLists.txt

+2-1
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,8 @@ find_package(Boost REQUIRED)
99
ament_auto_add_library(tier4_autoware_utils SHARED
1010
src/tier4_autoware_utils.cpp
1111
src/planning/planning_marker_helper.cpp
12-
src/vehicle/vehicle_state_checker.cpp
12+
src/vehicle/vehicle_stop_checker.cpp
13+
src/vehicle/vehicle_arrival_checker.cpp
1314
)
1415

1516
if(BUILD_TESTING)

‎common/tier4_autoware_utils/include/tier4_autoware_utils/vehicle/vehicle_state_checker.hpp ‎common/tier4_autoware_utils/include/tier4_autoware_utils/vehicle/vehicle_arrival_checker.hpp

+8-15
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2022 Tier IV, Inc.
1+
// Copyright 2022 TIER IV, Inc.
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -12,13 +12,12 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef TIER4_AUTOWARE_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_
16-
#define TIER4_AUTOWARE_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_
15+
#ifndef TIER4_AUTOWARE_UTILS__VEHICLE__VEHICLE_ARRIVAL_CHECKER_HPP_
16+
#define TIER4_AUTOWARE_UTILS__VEHICLE__VEHICLE_ARRIVAL_CHECKER_HPP_
1717

1818
#include <rclcpp/rclcpp.hpp>
1919

2020
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
21-
#include <autoware_auto_vehicle_msgs/msg/velocity_report.hpp>
2221
#include <geometry_msgs/msg/twist_stamped.hpp>
2322
#include <nav_msgs/msg/odometry.hpp>
2423

@@ -29,24 +28,20 @@ namespace tier4_autoware_utils
2928
{
3029

3130
using autoware_auto_planning_msgs::msg::Trajectory;
32-
using autoware_auto_vehicle_msgs::msg::VelocityReport;
3331

34-
class VehicleStateChecker
32+
class VehicleArrivalChecker
3533
{
3634
public:
37-
explicit VehicleStateChecker(rclcpp::Node * node);
35+
explicit VehicleArrivalChecker(rclcpp::Node * node);
3836

39-
bool checkStopped(const double stop_duration) const;
37+
bool isVehicleStopped(const double stop_duration) const;
4038

41-
bool checkStoppedAtStopPoint(const double stop_duration) const;
42-
43-
bool checkStoppedWithoutLocalization(const double stop_duration) const;
39+
bool isVehicleStoppedAtStopPoint(const double stop_duration) const;
4440

4541
rclcpp::Logger getLogger() { return logger_; }
4642

4743
private:
4844
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
49-
rclcpp::Subscription<VelocityReport>::SharedPtr sub_velocity_status_;
5045
rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
5146

5247
rclcpp::Clock::SharedPtr clock_;
@@ -56,15 +51,13 @@ class VehicleStateChecker
5651
Trajectory::SharedPtr trajectory_ptr_;
5752

5853
std::deque<geometry_msgs::msg::TwistStamped> twist_buffer_;
59-
std::deque<VelocityReport> velocity_status_buffer_;
6054

6155
static constexpr double velocity_buffer_time_sec = 10.0;
6256
static constexpr double th_arrived_distance_m = 1.0;
6357

6458
void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg);
65-
void onVelocityStatus(const VelocityReport::SharedPtr msg);
6659
void onTrajectory(const Trajectory::SharedPtr msg);
6760
};
6861
} // namespace tier4_autoware_utils
6962

70-
#endif // TIER4_AUTOWARE_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_
63+
#endif // TIER4_AUTOWARE_UTILS__VEHICLE__VEHICLE_ARRIVAL_CHECKER_HPP_

‎common/tier4_autoware_utils/include/tier4_autoware_utils/vehicle/vehicle_stop_checker.hpp

+52
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
// Copyright 2022 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef TIER4_AUTOWARE_UTILS__VEHICLE__VEHICLE_STOP_CHECKER_HPP_
16+
#define TIER4_AUTOWARE_UTILS__VEHICLE__VEHICLE_STOP_CHECKER_HPP_
17+
18+
#include <rclcpp/rclcpp.hpp>
19+
20+
#include <geometry_msgs/msg/twist_stamped.hpp>
21+
#include <nav_msgs/msg/odometry.hpp>
22+
23+
#include <deque>
24+
#include <memory>
25+
26+
namespace tier4_autoware_utils
27+
{
28+
29+
class VehicleStopChecker
30+
{
31+
public:
32+
explicit VehicleStopChecker(rclcpp::Node * node);
33+
34+
bool isVehicleStopped(const double stop_duration) const;
35+
36+
rclcpp::Logger getLogger() { return logger_; }
37+
38+
private:
39+
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
40+
41+
rclcpp::Clock::SharedPtr clock_;
42+
rclcpp::Logger logger_;
43+
44+
std::deque<geometry_msgs::msg::TwistStamped> twist_buffer_;
45+
46+
static constexpr double velocity_buffer_time_sec = 10.0;
47+
48+
void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg);
49+
};
50+
} // namespace tier4_autoware_utils
51+
52+
#endif // TIER4_AUTOWARE_UTILS__VEHICLE__VEHICLE_STOP_CHECKER_HPP_

‎common/tier4_autoware_utils/src/vehicle/vehicle_state_checker.cpp ‎common/tier4_autoware_utils/src/vehicle/vehicle_arrival_checker.cpp

+10-62
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
// Copyright 2022 Tier IV, Inc.
1+
// Copyright 2022 TIER IV, Inc.
22
//
33
// Licensed under the Apache License, Version 2.0 (the "License");
44
// you may not use this file except in compliance with the License.
@@ -12,33 +12,29 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "tier4_autoware_utils/vehicle/vehicle_state_checker.hpp"
15+
#include "tier4_autoware_utils/vehicle/vehicle_arrival_checker.hpp"
1616

1717
#include "tier4_autoware_utils/trajectory/trajectory.hpp"
1818

1919
#include <string>
2020

2121
namespace tier4_autoware_utils
2222
{
23-
VehicleStateChecker::VehicleStateChecker(rclcpp::Node * node)
23+
VehicleArrivalChecker::VehicleArrivalChecker(rclcpp::Node * node)
2424
: clock_(node->get_clock()), logger_(node->get_logger())
2525
{
2626
using std::placeholders::_1;
2727

2828
sub_odom_ = node->create_subscription<nav_msgs::msg::Odometry>(
2929
"/localization/kinematic_state", rclcpp::QoS(1),
30-
std::bind(&VehicleStateChecker::onOdom, this, _1));
30+
std::bind(&VehicleArrivalChecker::onOdom, this, _1));
3131

3232
sub_trajectory_ = node->create_subscription<Trajectory>(
3333
"/planning/scenario_planning/trajectory", rclcpp::QoS(1),
34-
std::bind(&VehicleStateChecker::onTrajectory, this, _1));
35-
36-
sub_velocity_status_ = node->create_subscription<VelocityReport>(
37-
"/vehicle/status/velocity_status", rclcpp::QoS(1),
38-
std::bind(&VehicleStateChecker::onVelocityStatus, this, _1));
34+
std::bind(&VehicleArrivalChecker::onTrajectory, this, _1));
3935
}
4036

41-
bool VehicleStateChecker::checkStopped(const double stop_duration = 0.0) const
37+
bool VehicleArrivalChecker::isVehicleStopped(const double stop_duration = 0.0) const
4238
{
4339
if (twist_buffer_.empty()) {
4440
return false;
@@ -67,13 +63,13 @@ bool VehicleStateChecker::checkStopped(const double stop_duration = 0.0) const
6763
return true;
6864
}
6965

70-
bool VehicleStateChecker::checkStoppedAtStopPoint(const double stop_duration = 0.0) const
66+
bool VehicleArrivalChecker::isVehicleStoppedAtStopPoint(const double stop_duration = 0.0) const
7167
{
7268
if (!odometry_ptr_ || !trajectory_ptr_) {
7369
return false;
7470
}
7571

76-
if (!checkStopped(stop_duration)) {
72+
if (!isVehicleStopped(stop_duration)) {
7773
return false;
7874
}
7975

@@ -88,36 +84,7 @@ bool VehicleStateChecker::checkStoppedAtStopPoint(const double stop_duration = 0
8884
th_arrived_distance_m;
8985
}
9086

91-
bool VehicleStateChecker::checkStoppedWithoutLocalization(const double stop_duration = 0.0) const
92-
{
93-
if (velocity_status_buffer_.empty()) {
94-
return false;
95-
}
96-
97-
// Get velocities within stop_duration
98-
const auto now = clock_->now();
99-
std::vector<double> vs;
100-
for (const auto & velocity : velocity_status_buffer_) {
101-
vs.push_back(velocity.longitudinal_velocity);
102-
103-
const auto time_diff = now - velocity.header.stamp;
104-
if (time_diff.seconds() >= stop_duration) {
105-
break;
106-
}
107-
}
108-
109-
// Check all velocities
110-
constexpr double stop_velocity = 1e-3;
111-
for (const auto & v : vs) {
112-
if (v >= stop_velocity) {
113-
return false;
114-
}
115-
}
116-
117-
return true;
118-
}
119-
120-
void VehicleStateChecker::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg)
87+
void VehicleArrivalChecker::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg)
12188
{
12289
odometry_ptr_ = msg;
12390

@@ -142,24 +109,5 @@ void VehicleStateChecker::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg)
142109
}
143110
}
144111

145-
void VehicleStateChecker::onVelocityStatus(const VelocityReport::SharedPtr msg)
146-
{
147-
velocity_status_buffer_.push_front(*msg);
148-
149-
const auto now = clock_->now();
150-
while (true) {
151-
// Check oldest data time
152-
const auto time_diff = now - velocity_status_buffer_.back().header.stamp;
153-
154-
// Finish when oldest data is newer than threshold
155-
if (time_diff.seconds() <= velocity_buffer_time_sec) {
156-
break;
157-
}
158-
159-
// Remove old data
160-
velocity_status_buffer_.pop_back();
161-
}
162-
}
163-
164-
void VehicleStateChecker::onTrajectory(const Trajectory::SharedPtr msg) { trajectory_ptr_ = msg; }
112+
void VehicleArrivalChecker::onTrajectory(const Trajectory::SharedPtr msg) { trajectory_ptr_ = msg; }
165113
} // namespace tier4_autoware_utils

‎common/tier4_autoware_utils/src/vehicle/vehicle_stop_checker.cpp

+82
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,82 @@
1+
// Copyright 2022 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "tier4_autoware_utils/vehicle/vehicle_stop_checker.hpp"
16+
17+
#include <string>
18+
19+
namespace tier4_autoware_utils
20+
{
21+
VehicleStopChecker::VehicleStopChecker(rclcpp::Node * node)
22+
: clock_(node->get_clock()), logger_(node->get_logger())
23+
{
24+
using std::placeholders::_1;
25+
26+
sub_odom_ = node->create_subscription<nav_msgs::msg::Odometry>(
27+
"/localization/kinematic_state", rclcpp::QoS(1),
28+
std::bind(&VehicleStopChecker::onOdom, this, _1));
29+
}
30+
31+
bool VehicleStopChecker::isVehicleStopped(const double stop_duration = 0.0) const
32+
{
33+
if (twist_buffer_.empty()) {
34+
return false;
35+
}
36+
37+
// Get velocities within stop_duration
38+
const auto now = clock_->now();
39+
std::vector<double> vs;
40+
for (const auto & velocity : twist_buffer_) {
41+
vs.push_back(velocity.twist.linear.x);
42+
43+
const auto time_diff = now - velocity.header.stamp;
44+
if (time_diff.seconds() >= stop_duration) {
45+
break;
46+
}
47+
}
48+
49+
// Check all velocities
50+
constexpr double stop_velocity = 1e-3;
51+
for (const auto & v : vs) {
52+
if (v >= stop_velocity) {
53+
return false;
54+
}
55+
}
56+
57+
return true;
58+
}
59+
60+
void VehicleStopChecker::onOdom(const nav_msgs::msg::Odometry::SharedPtr msg)
61+
{
62+
auto current_velocity = std::make_shared<geometry_msgs::msg::TwistStamped>();
63+
current_velocity->header = msg->header;
64+
current_velocity->twist = msg->twist.twist;
65+
66+
twist_buffer_.push_front(*current_velocity);
67+
68+
const auto now = clock_->now();
69+
while (true) {
70+
// Check oldest data time
71+
const auto time_diff = now - twist_buffer_.back().header.stamp;
72+
73+
// Finish when oldest data is newer than threshold
74+
if (time_diff.seconds() <= velocity_buffer_time_sec) {
75+
break;
76+
}
77+
78+
// Remove old data
79+
twist_buffer_.pop_back();
80+
}
81+
}
82+
} // namespace tier4_autoware_utils

0 commit comments

Comments
 (0)