Skip to content

Commit fb3d710

Browse files
satoshi-otashmpwk
satoshi-ota
authored and
shmpwk
committed
feat(tier4_autoware_utils): add vehicle state checker (autowarefoundation#896)
* feat(tier4_autoware_utils): add vehicle state checker Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(tier4_autoware_utils): use absolute value Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(tier4_autoware_utils): divide into two classies Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * test(tier4_autoware_utils): add unit test for vehicle_state checker Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(tier4_autoware_utils): impl class inheritance Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * docs(tier4_autoware_utils): add vehicle_state_checker document Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(tier4_autoware_utils): into same loop Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(tier4_autoware_utils): fix variables name Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(tier4_autoware_utils): remove redundant codes Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> Signed-off-by: Shumpei Wakabayashi <shumpei.wakabayashi@tier4.jp>
1 parent 646ea94 commit fb3d710

File tree

8 files changed

+896
-1
lines changed

8 files changed

+896
-1
lines changed

‎common/tier4_autoware_utils/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@ 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
1213
)
1314

1415
if(BUILD_TESTING)

‎common/tier4_autoware_utils/docs/vehicle/vehicle.md

+169
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,169 @@
1+
# vehicle utils
2+
3+
Vehicle utils provides a convenient library used to check vehicle status.
4+
5+
## Feature
6+
7+
The library contains following classes.
8+
9+
### vehicle_stop_checker
10+
11+
This class check whether the vehicle is stopped or not based on localization result.
12+
13+
#### Subscribed Topics
14+
15+
| Name | Type | Description |
16+
| ------------------------------- | ------------------------- | ---------------- |
17+
| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | vehicle odometry |
18+
19+
#### Parameters
20+
21+
| Name | Type | Default Value | Explanation |
22+
| -------------------------- | ------ | ------------- | --------------------------- |
23+
| `velocity_buffer_time_sec` | double | 10.0 | odometry buffering time [s] |
24+
25+
#### Member functions
26+
27+
```c++
28+
bool isVehicleStopped(const double stop_duration)
29+
```
30+
31+
- Check simply whether the vehicle is stopped based on the localization result.
32+
- Returns `true` if the vehicle is stopped, even if system outputs a non-zero target velocity.
33+
34+
#### Example Usage
35+
36+
Necessary includes:
37+
38+
```c++
39+
#include <tier4_autoware_utils/vehicle/vehicle_state_checker.hpp>
40+
```
41+
42+
1.Create a checker instance.
43+
44+
```c++
45+
class SampleNode : public rclcpp::Node
46+
{
47+
public:
48+
SampleNode() : Node("sample_node")
49+
{
50+
vehicle_stop_checker_ = std::make_unique<VehicleStopChecker>(this);
51+
}
52+
53+
std::unique_ptr<VehicleStopChecker> vehicle_stop_checker_;
54+
55+
bool sampleFunc();
56+
57+
...
58+
}
59+
```
60+
61+
2.Check the vehicle state.
62+
63+
```c++
64+
65+
bool SampleNode::sampleFunc()
66+
{
67+
...
68+
69+
const auto result_1 = vehicle_stop_checker_->isVehicleStopped();
70+
71+
...
72+
73+
const auto result_2 = vehicle_stop_checker_->isVehicleStopped(3.0);
74+
75+
...
76+
}
77+
78+
```
79+
80+
### vehicle_arrival_checker
81+
82+
This class check whether the vehicle arrive at stop point based on localization and planning result.
83+
84+
#### Subscribed Topics
85+
86+
| Name | Type | Description |
87+
| ---------------------------------------- | ---------------------------------------------- | ---------------- |
88+
| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | vehicle odometry |
89+
| `/planning/scenario_planning/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | trajectory |
90+
91+
#### Parameters
92+
93+
| Name | Type | Default Value | Explanation |
94+
| -------------------------- | ------ | ------------- | ---------------------------------------------------------------------- |
95+
| `velocity_buffer_time_sec` | double | 10.0 | odometry buffering time [s] |
96+
| `th_arrived_distance_m` | double | 1.0 | threshold distance to check if vehicle has arrived at target point [m] |
97+
98+
#### Member functions
99+
100+
```c++
101+
bool isVehicleStopped(const double stop_duration)
102+
```
103+
104+
- Check simply whether the vehicle is stopped based on the localization result.
105+
- Returns `true` if the vehicle is stopped, even if system outputs a non-zero target velocity.
106+
107+
```c++
108+
bool isVehicleStoppedAtStopPoint(const double stop_duration)
109+
```
110+
111+
- Check whether the vehicle is stopped at stop point based on the localization and planning result.
112+
- Returns `true` if the vehicle is not only stopped but also arrived at stop point.
113+
114+
#### Example Usage
115+
116+
Necessary includes:
117+
118+
```c++
119+
#include <tier4_autoware_utils/vehicle/vehicle_state_checker.hpp>
120+
```
121+
122+
1.Create a checker instance.
123+
124+
```c++
125+
class SampleNode : public rclcpp::Node
126+
{
127+
public:
128+
SampleNode() : Node("sample_node")
129+
{
130+
vehicle_arrival_checker_ = std::make_unique<VehicleArrivalChecker>(this);
131+
}
132+
133+
std::unique_ptr<VehicleArrivalChecker> vehicle_arrival_checker_;
134+
135+
bool sampleFunc();
136+
137+
...
138+
}
139+
```
140+
141+
2.Check the vehicle state.
142+
143+
```c++
144+
145+
bool SampleNode::sampleFunc1()
146+
{
147+
...
148+
149+
const auto result_1 = vehicle_arrival_checker_->isVehicleStopped();
150+
151+
...
152+
153+
const auto result_2 = vehicle_arrival_checker_->isVehicleStopped(3.0);
154+
155+
...
156+
157+
const auto result_3 = vehicle_arrival_checker_->isVehicleStoppedAtStopPoint();
158+
159+
...
160+
161+
const auto result_4 = vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(3.0);
162+
163+
...
164+
}
165+
```
166+
167+
## Assumptions / Known limits
168+
169+
`vehicle_stop_checker` and `vehicle_arrival_checker` cannot check whether the vehicle is stopped more than `velocity_buffer_time_sec` second.

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

+76
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,76 @@
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_STATE_CHECKER_HPP_
16+
#define TIER4_AUTOWARE_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_
17+
18+
#include <rclcpp/rclcpp.hpp>
19+
20+
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
21+
#include <geometry_msgs/msg/twist_stamped.hpp>
22+
#include <nav_msgs/msg/odometry.hpp>
23+
24+
#include <deque>
25+
#include <memory>
26+
27+
namespace tier4_autoware_utils
28+
{
29+
30+
using autoware_auto_planning_msgs::msg::Trajectory;
31+
using geometry_msgs::msg::TwistStamped;
32+
using nav_msgs::msg::Odometry;
33+
34+
class VehicleStopChecker
35+
{
36+
public:
37+
explicit VehicleStopChecker(rclcpp::Node * node);
38+
39+
bool isVehicleStopped(const double stop_duration) const;
40+
41+
rclcpp::Logger getLogger() { return logger_; }
42+
43+
protected:
44+
rclcpp::Subscription<Odometry>::SharedPtr sub_odom_;
45+
rclcpp::Clock::SharedPtr clock_;
46+
rclcpp::Logger logger_;
47+
48+
Odometry::SharedPtr odometry_ptr_;
49+
50+
std::deque<TwistStamped> twist_buffer_;
51+
52+
private:
53+
static constexpr double velocity_buffer_time_sec = 10.0;
54+
55+
void onOdom(const Odometry::SharedPtr msg);
56+
};
57+
58+
class VehicleArrivalChecker : public VehicleStopChecker
59+
{
60+
public:
61+
explicit VehicleArrivalChecker(rclcpp::Node * node);
62+
63+
bool isVehicleStoppedAtStopPoint(const double stop_duration) const;
64+
65+
private:
66+
static constexpr double th_arrived_distance_m = 1.0;
67+
68+
rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
69+
70+
Trajectory::SharedPtr trajectory_ptr_;
71+
72+
void onTrajectory(const Trajectory::SharedPtr msg);
73+
};
74+
} // namespace tier4_autoware_utils
75+
76+
#endif // TIER4_AUTOWARE_UTILS__VEHICLE__VEHICLE_STATE_CHECKER_HPP_

‎common/tier4_autoware_utils/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@
1313
<build_depend>autoware_cmake</build_depend>
1414

1515
<depend>autoware_auto_planning_msgs</depend>
16+
<depend>autoware_auto_vehicle_msgs</depend>
1617
<depend>builtin_interfaces</depend>
1718
<depend>diagnostic_msgs</depend>
1819
<depend>geometry_msgs</depend>

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

+118
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,118 @@
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_state_checker.hpp"
16+
17+
#include "tier4_autoware_utils/trajectory/trajectory.hpp"
18+
19+
#include <string>
20+
21+
namespace tier4_autoware_utils
22+
{
23+
VehicleStopChecker::VehicleStopChecker(rclcpp::Node * node)
24+
: clock_(node->get_clock()), logger_(node->get_logger())
25+
{
26+
using std::placeholders::_1;
27+
28+
sub_odom_ = node->create_subscription<Odometry>(
29+
"/localization/kinematic_state", rclcpp::QoS(1),
30+
std::bind(&VehicleStopChecker::onOdom, this, _1));
31+
}
32+
33+
bool VehicleStopChecker::isVehicleStopped(const double stop_duration = 0.0) const
34+
{
35+
if (twist_buffer_.empty()) {
36+
return false;
37+
}
38+
39+
constexpr double stop_velocity = 1e-3;
40+
const auto now = clock_->now();
41+
42+
const auto time_buffer_back = now - twist_buffer_.back().header.stamp;
43+
if (time_buffer_back.seconds() < stop_duration) {
44+
return false;
45+
}
46+
47+
// Get velocities within stop_duration
48+
for (const auto & velocity : twist_buffer_) {
49+
if (stop_velocity <= velocity.twist.linear.x) {
50+
return false;
51+
}
52+
53+
const auto time_diff = now - velocity.header.stamp;
54+
if (time_diff.seconds() >= stop_duration) {
55+
break;
56+
}
57+
}
58+
59+
return true;
60+
}
61+
62+
void VehicleStopChecker::onOdom(const Odometry::SharedPtr msg)
63+
{
64+
odometry_ptr_ = msg;
65+
66+
auto current_velocity = std::make_shared<TwistStamped>();
67+
current_velocity->header = msg->header;
68+
current_velocity->twist = msg->twist.twist;
69+
70+
twist_buffer_.push_front(*current_velocity);
71+
72+
const auto now = clock_->now();
73+
while (true) {
74+
// Check oldest data time
75+
const auto time_diff = now - twist_buffer_.back().header.stamp;
76+
77+
// Finish when oldest data is newer than threshold
78+
if (time_diff.seconds() <= velocity_buffer_time_sec) {
79+
break;
80+
}
81+
82+
// Remove old data
83+
twist_buffer_.pop_back();
84+
}
85+
}
86+
87+
VehicleArrivalChecker::VehicleArrivalChecker(rclcpp::Node * node) : VehicleStopChecker(node)
88+
{
89+
using std::placeholders::_1;
90+
91+
sub_trajectory_ = node->create_subscription<Trajectory>(
92+
"/planning/scenario_planning/trajectory", rclcpp::QoS(1),
93+
std::bind(&VehicleArrivalChecker::onTrajectory, this, _1));
94+
}
95+
96+
bool VehicleArrivalChecker::isVehicleStoppedAtStopPoint(const double stop_duration = 0.0) const
97+
{
98+
if (!odometry_ptr_ || !trajectory_ptr_) {
99+
return false;
100+
}
101+
102+
if (!isVehicleStopped(stop_duration)) {
103+
return false;
104+
}
105+
106+
const auto & p = odometry_ptr_->pose.pose.position;
107+
const auto idx = searchZeroVelocityIndex(trajectory_ptr_->points);
108+
109+
if (!idx) {
110+
return false;
111+
}
112+
113+
return std::abs(calcSignedArcLength(trajectory_ptr_->points, p, idx.get())) <
114+
th_arrived_distance_m;
115+
}
116+
117+
void VehicleArrivalChecker::onTrajectory(const Trajectory::SharedPtr msg) { trajectory_ptr_ = msg; }
118+
} // namespace tier4_autoware_utils

‎common/tier4_autoware_utils/test/src/test_autoware_utils.cpp

+4-1
Original file line numberDiff line numberDiff line change
@@ -19,5 +19,8 @@
1919
int main(int argc, char * argv[])
2020
{
2121
testing::InitGoogleTest(&argc, argv);
22-
return RUN_ALL_TESTS();
22+
rclcpp::init(argc, argv);
23+
bool result = RUN_ALL_TESTS();
24+
rclcpp::shutdown();
25+
return result;
2326
}

0 commit comments

Comments
 (0)