Skip to content

Commit 2c80e30

Browse files
satoshi-otasatoshi-ota
authored
feat(driving_environment_analyzer): add rviz plugin (#23)
* feat(driving_environment_analyzer): add rviz plugin Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(driving_environment_analyzer): output csv file Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent eb7cf14 commit 2c80e30

23 files changed

+1600
-472
lines changed

‎__pycache__/mkdocs_macros.cpython-310.pyc

2.02 KB
Binary file not shown.

‎driving_environment_analyzer/CMakeLists.txt

+18-5
Original file line numberDiff line numberDiff line change
@@ -3,17 +3,30 @@ project(driving_environment_analyzer)
33

44
find_package(autoware_cmake REQUIRED)
55
autoware_package()
6+
find_package(Qt5 REQUIRED Core Widgets)
7+
set(QT_LIBRARIES Qt5::Widgets)
8+
set(CMAKE_AUTOMOC ON)
9+
set(CMAKE_INCLUDE_CURRENT_DIR ON)
610

7-
ament_auto_add_library(${PROJECT_NAME}_node SHARED
11+
ament_auto_add_library(${PROJECT_NAME} SHARED
12+
include/${PROJECT_NAME}/driving_environment_analyzer_node.hpp
13+
include/${PROJECT_NAME}/driving_environment_analyzer_rviz_plugin.hpp
814
DIRECTORY src
915
)
1016

11-
rclcpp_components_register_node(${PROJECT_NAME}_node
12-
PLUGIN "driving_environment_analyzer::DrivingEnvironmentAnalyzer"
13-
EXECUTABLE driving_environment_analyzer
17+
target_link_libraries(${PROJECT_NAME}
18+
${QT_LIBRARIES}
1419
)
1520

21+
rclcpp_components_register_node(${PROJECT_NAME}
22+
PLUGIN "driving_environment_analyzer::DrivingEnvironmentAnalyzerNode"
23+
EXECUTABLE driving_environment_analyzer_node
24+
)
25+
26+
pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
27+
1628
ament_auto_package(
1729
INSTALL_TO_SHARE
18-
launch
30+
launch
31+
plugins
1932
)

‎driving_environment_analyzer/README.md

+88-1
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,94 @@
22

33
このツールはROSBAGに含まれる走行履歴を元に走行環境のODDを解析するツールです。
44

5-
## How to use
5+
## ROSBAGの特定時刻における周囲のODDを解析する場合
6+
7+
この場合にはRvizプラグインである`driving_environment_analyzer_rviz_panel`を使用することをおすすめします。
8+
9+
現在以下の情報が出力可能です。
10+
11+
- EGOの現在車速
12+
- 現在位置の勾配
13+
- EGOの挙動
14+
- 現在の車線情報
15+
16+
こちらのツールはautoware_launchに含まれる`logging_simulator`を使用します。まず以下のコマンドからシミュレータを起動してください。
17+
18+
`ros2 launch autoware_launch logging_simulator.launch.xml map_path:=<MAP> vehicle_model:=<VEHICLE_NAME> sensor_model:=<AIP_NAME> sensing:=false control:=false planning:=false perception:=false localization:=false system:=false`
19+
20+
![fig1](./images/launch_rviz.png)
21+
22+
シミュレータ起動時に地図を読み込むためROSBAGに地図情報が含まれていなくてもODDの解析が可能です。（ただし、その場合にはROSBAG取得の際に使用した地図を指定してシミュレータを起動するようにしてください。）
23+
24+
次に本パッケージに含まれる解析ツールを起動します。Rviz画面左上部のAdd New PanelからDrivingEnvironmentAnalyzerPanelを選択してください。これでRviz左下に新しく操作パネルが追加されます。
25+
26+
![fig1](./images/launch_tool.png)
27+
![fig1](./images/rviz_overview_1.png)
28+
29+
本ツールはROSBAGファイル指定してロードできる他、複数のROSBAGファイルが格納されているディレクトリを指定することも可能です。ただし、その場合には事前に以下のコマンドで`metadata.yaml`の生成が必要になります。
30+
31+
`ros2 bag reindex <DIR_PATH> sqlite3`
32+
33+
![fig1](./images/select_directory.png)
34+
35+
ROSBAGの読み込みが完了したらODDを解析したい時刻を指定します。時刻の指定にはUnix timeを直接指定するほかスライドバーも使用可能です。左に表示されている日時を参考に調整してください。
36+
37+
![fig1](./images/select_time .png)
38+
39+
また、このときViewsのTarget Flameを`base_link`にしておくことで、指定した時刻のEGOの位置と周囲の状況をRvizで可視化可能です。
40+
41+
![fig1](./images/select_target_frame.png)
42+
43+
時刻の指定が完了したら、`Set time stamp`ボタンを押し、最後に`Analyze dynamic ODD factor`を押すことで解析が始まります。
44+
45+
![fig1](./images/rviz_overview_2.png)
46+
47+
```bash
48+
[rviz2-11] ***********************************************************
49+
[rviz2-11] ODD analysis result
50+
[rviz2-11] ***********************************************************
51+
[rviz2-11] Type: TIME SPECIFIED
52+
[rviz2-11] Time: 2024-04-22 14:48:05
53+
[rviz2-11]
54+
[rviz2-11]
55+
[rviz2-11] - EGO INFO
56+
[rviz2-11] [SPEED] : 0 [m/s]
57+
[rviz2-11] [ELEVATION ANGLE] : 0.00963597 [rad]
58+
[rviz2-11]
59+
[rviz2-11] - EGO BEHAIOVR
60+
[rviz2-11] [AVOIDANCE(R)] : NONE
61+
[rviz2-11] [AVOIDANCE(L)] : NONE
62+
[rviz2-11] [LANE_CHANGE(R)] : NONE
63+
[rviz2-11] [LANE_CHANGE(L)] : NONE
64+
[rviz2-11] [START_PLANNER] : SAFE: true COMMAND: deactivate
65+
[rviz2-11] [GOAL_PLANNER] : NONE
66+
[rviz2-11] [CROSSWALK] : NONE
67+
[rviz2-11] [INTERSECTION] : NONE
68+
[rviz2-11]
69+
[rviz2-11] - LANE INFO
70+
[rviz2-11] [ID] : 176126
71+
[rviz2-11] [WIDTH] : 4.24132 [m]
72+
[rviz2-11] [SHAPE] : STRAIGHT
73+
[rviz2-11] [RIGHT LANE NUM] : 0
74+
[rviz2-11] [LEFT LANE NUM] : 0
75+
[rviz2-11] [TOTAL LANE NUM] : 1
76+
[rviz2-11] [SAME DIRECTION LANE] : NONE
77+
[rviz2-11] [OPPOSITE DIRECTION LANE] : NONE
78+
[rviz2-11] [ROAD SHOULDER] : EXIST
79+
[rviz2-11]
80+
[rviz2-11] - SURROUND OBJECT NUM
81+
[rviz2-11] [UNKNOWN] : 0
82+
[rviz2-11] [CAR] : 6
83+
[rviz2-11] [TRUCK] : 0
84+
[rviz2-11] [BUS] : 3
85+
[rviz2-11] [TRAILER] : 2
86+
[rviz2-11] [MOTORCYCLE] : 0
87+
[rviz2-11] [BICYCLE] : 0
88+
[rviz2-11] [PEDESTRIAN] : 7
89+
[rviz2-11] ***********************************************************
90+
```
91+
92+
## ROSBAG全体に対して経路沿いのODDを解析する場合
693

794
現在以下の情報が出力可能です。
895

‎driving_environment_analyzer/images/launch_rviz.png

296 KB
Loading

‎driving_environment_analyzer/images/launch_tool.png

83.6 KB
Loading

‎driving_environment_analyzer/images/rviz_overview_1.png

290 KB
Loading

‎driving_environment_analyzer/images/rviz_overview_2.png

506 KB
Loading

‎driving_environment_analyzer/images/select_directory.png

45.1 KB
Loading

‎driving_environment_analyzer/images/select_target_frame.png

27.2 KB
Loading

‎driving_environment_analyzer/images/select_time.png

34.7 KB
Loading

‎driving_environment_analyzer/include/driving_environment_analyzer/analyzer_core.hpp

+103
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,103 @@
1+
// Copyright 2024 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 DRIVING_ENVIRONMENT_ANALYZER__ANALYZER_CORE_HPP_
16+
#define DRIVING_ENVIRONMENT_ANALYZER__ANALYZER_CORE_HPP_
17+
18+
#include "driving_environment_analyzer/type_alias.hpp"
19+
#include "rosbag2_cpp/reader.hpp"
20+
21+
#include <rclcpp/rclcpp.hpp>
22+
#include <route_handler/route_handler.hpp>
23+
24+
#include <memory>
25+
#include <string>
26+
#include <utility>
27+
#include <vector>
28+
29+
namespace driving_environment_analyzer::analyzer_core
30+
{
31+
32+
struct ODDRawData
33+
{
34+
rcutils_time_point_value_t timestamp;
35+
Odometry odometry;
36+
PredictedObjects objects;
37+
TFMessage tf;
38+
TFMessage tf_static;
39+
CooperateStatusArray rtc_status;
40+
};
41+
42+
class AnalyzerCore
43+
{
44+
public:
45+
explicit AnalyzerCore(rclcpp::Node & node);
46+
~AnalyzerCore();
47+
48+
bool isDataReadyForStaticODDAnalysis() const;
49+
bool isDataReadyForDynamicODDAnalysis() const { return odd_raw_data_.has_value(); }
50+
51+
void analyzeStaticODDFactor() const;
52+
void analyzeDynamicODDFactor(std::ofstream & ofs_csv_file) const;
53+
54+
void addHeader(std::ofstream & ofs_csv_file) const;
55+
56+
void setBagFile(const std::string & file_name);
57+
58+
void setTimeStamp(const rcutils_time_point_value_t & timestamp)
59+
{
60+
odd_raw_data_ = getRawData(timestamp);
61+
}
62+
63+
void setMap(const HADMapBin & msg) { route_handler_.setMap(msg); }
64+
65+
void clearData() { odd_raw_data_ = std::nullopt; }
66+
67+
std::pair<rcutils_time_point_value_t, rcutils_time_point_value_t> getBagStartEndTime()
68+
{
69+
const auto metadata = reader_.get_metadata();
70+
const auto start_time =
71+
duration_cast<seconds>(metadata.starting_time.time_since_epoch()).count();
72+
const auto duration_time = duration_cast<seconds>(metadata.duration).count();
73+
return {start_time, start_time + duration_time};
74+
}
75+
76+
Odometry getOdometry() const { return odd_raw_data_.value().odometry; }
77+
PredictedObjects getObjects() const { return odd_raw_data_.value().objects; }
78+
TFMessage getTF() const { return odd_raw_data_.value().tf; }
79+
TFMessage getTFStatic() const { return odd_raw_data_.value().tf_static; }
80+
81+
private:
82+
Pose getEgoPose() const { return odd_raw_data_.value().odometry.pose.pose; }
83+
84+
double getEgoSpeed() const { return odd_raw_data_.value().odometry.twist.twist.linear.x; }
85+
86+
template <class T>
87+
std::optional<T> getLastTopic(const std::string & topic_name);
88+
template <class T>
89+
std::optional<T> seekTopic(
90+
const std::string & topic_name, const rcutils_time_point_value_t & timestamp);
91+
std::optional<ODDRawData> getRawData(const rcutils_time_point_value_t & timestamp);
92+
93+
std::optional<ODDRawData> odd_raw_data_{std::nullopt};
94+
95+
route_handler::RouteHandler route_handler_;
96+
97+
rosbag2_cpp::Reader reader_;
98+
99+
rclcpp::Logger logger_;
100+
};
101+
} // namespace driving_environment_analyzer::analyzer_core
102+
103+
#endif // DRIVING_ENVIRONMENT_ANALYZER__ANALYZER_CORE_HPP_

‎driving_environment_analyzer/include/driving_environment_analyzer/driving_environment_analyzer_node.hpp

+45
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
// Copyright 2024 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 DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_NODE_HPP_
16+
#define DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_NODE_HPP_
17+
18+
#include "driving_environment_analyzer/analyzer_core.hpp"
19+
#include "driving_environment_analyzer/type_alias.hpp"
20+
21+
#include <memory>
22+
#include <string>
23+
#include <vector>
24+
25+
namespace driving_environment_analyzer
26+
{
27+
28+
class DrivingEnvironmentAnalyzerNode : public rclcpp::Node
29+
{
30+
public:
31+
explicit DrivingEnvironmentAnalyzerNode(const rclcpp::NodeOptions & node_options);
32+
33+
private:
34+
void onMap(const HADMapBin::ConstSharedPtr map_msg);
35+
void analyze();
36+
37+
std::shared_ptr<analyzer_core::AnalyzerCore> analyzer_;
38+
39+
rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
40+
rclcpp::TimerBase::SharedPtr timer_;
41+
rosbag2_cpp::Reader reader_;
42+
};
43+
} // namespace driving_environment_analyzer
44+
45+
#endif // DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_NODE_HPP_

‎driving_environment_analyzer/include/driving_environment_analyzer/driving_environment_analyzer_rviz_plugin.hpp

+86
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,86 @@
1+
// Copyright 2024 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 DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_RVIZ_PLUGIN_HPP_
16+
#define DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_RVIZ_PLUGIN_HPP_
17+
18+
#include "driving_environment_analyzer/analyzer_core.hpp"
19+
#include "driving_environment_analyzer/type_alias.hpp"
20+
21+
#include <QDir>
22+
#include <QFileDialog>
23+
#include <QHBoxLayout>
24+
#include <QLabel>
25+
#include <QPushButton>
26+
#include <QSlider>
27+
#include <QSpinBox>
28+
#include <rviz_common/display_context.hpp>
29+
#include <rviz_common/panel.hpp>
30+
#include <rviz_common/render_panel.hpp>
31+
#include <rviz_common/ros_integration/ros_node_abstraction_iface.hpp>
32+
#include <rviz_common/view_manager.hpp>
33+
#include <rviz_rendering/render_window.hpp>
34+
35+
#include <memory>
36+
#include <string>
37+
#include <vector>
38+
39+
namespace driving_environment_analyzer
40+
{
41+
42+
class DrivingEnvironmentAnalyzerPanel : public rviz_common::Panel
43+
{
44+
Q_OBJECT
45+
46+
public:
47+
explicit DrivingEnvironmentAnalyzerPanel(QWidget * parent = nullptr);
48+
~DrivingEnvironmentAnalyzerPanel() override;
49+
void onInitialize() override;
50+
51+
public Q_SLOTS:
52+
void onBoxUpdate();
53+
void onSliderUpdate();
54+
void onClickSetTimeStamp();
55+
void onSelectBagFile();
56+
void onSelectDirectory();
57+
void onClickAnalyzeStaticODDFactor();
58+
void onClickAnalyzeDynamicODDFactor();
59+
60+
private:
61+
void onMap(const HADMapBin::ConstSharedPtr map_msg);
62+
63+
std::shared_ptr<analyzer_core::AnalyzerCore> analyzer_;
64+
65+
std::ofstream ofs_csv_file_;
66+
67+
QSpinBox * bag_time_selector_;
68+
QSlider * bag_time_slider_;
69+
QLabel * bag_name_label_;
70+
QLabel * bag_time_line_;
71+
QPushButton * dir_button_ptr_;
72+
QPushButton * file_button_ptr_;
73+
QPushButton * analyze_static_odd_button_;
74+
QPushButton * analyze_dynamic_odd_button_;
75+
QPushButton * set_timestamp_btn_;
76+
77+
rclcpp::Node::SharedPtr raw_node_;
78+
rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
79+
rclcpp::Publisher<Odometry>::SharedPtr pub_odometry_;
80+
rclcpp::Publisher<PredictedObjects>::SharedPtr pub_objects_;
81+
rclcpp::Publisher<TFMessage>::SharedPtr pub_tf_;
82+
rclcpp::Publisher<TFMessage>::SharedPtr pub_tf_static_;
83+
};
84+
} // namespace driving_environment_analyzer
85+
86+
#endif // DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_RVIZ_PLUGIN_HPP_

0 commit comments

Comments
 (0)