feat(driving_environment_analyzer): add rviz plugin

Signed-off-by: satoshi-ota <[email protected]>

driving_environment_analyzer/CMakeLists.txt

@@ -3,17 +3,30 @@ project(driving_environment_analyzer)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
+find_package(Qt5 REQUIRED Core Widgets)
+set(QT_LIBRARIES Qt5::Widgets)
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
 
-ament_auto_add_library(${PROJECT_NAME}_node SHARED
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  include/${PROJECT_NAME}/driving_environment_analyzer_node.hpp
+  include/${PROJECT_NAME}/driving_environment_analyzer_rviz_plugin.hpp
   DIRECTORY src
 )
 
-rclcpp_components_register_node(${PROJECT_NAME}_node
-  PLUGIN "driving_environment_analyzer::DrivingEnvironmentAnalyzer"
-  EXECUTABLE driving_environment_analyzer
+target_link_libraries(${PROJECT_NAME}
+  ${QT_LIBRARIES}
 )
 
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "driving_environment_analyzer::DrivingEnvironmentAnalyzerNode"
+  EXECUTABLE driving_environment_analyzer_node
+)
+
+pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
+
 ament_auto_package(
   INSTALL_TO_SHARE
-    launch
+  launch
+  plugins
 )

driving_environment_analyzer/README.md

@@ -2,7 +2,94 @@
 
 このツールはROSBAGに含まれる走行履歴を元に走行環境のODDを解析するツールです。
 
-## How to use
+## ROSBAGの特定時刻における周囲のODDを解析する場合
+
+この場合にはRvizプラグインである`driving_environment_analyzer_rviz_panel`を使用することをおすすめします。
+
+現在以下の情報が出力可能です。
+
+- EGOの現在車速
+- 現在位置の勾配
+- EGOの挙動
+- 現在の車線情報
+
+こちらのツールはautoware_launchに含まれる`logging_simulator`を使用します。まず以下のコマンドからシミュレータを起動してください。
+
+`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`
+
+![fig1](./images/launch_rviz.png)
+
+シミュレータ起動時に地図を読み込むためROSBAGに地図情報が含まれていなくてもODDの解析が可能です。（ただし、その場合にはROSBAG取得の際に使用した地図を指定してシミュレータを起動するようにしてください。）
+
+次に本パッケージに含まれる解析ツールを起動します。Rviz画面左上部のAdd New PanelからDrivingEnvironmentAnalyzerPanelを選択してください。これでRviz左下に新しく操作パネルが追加されます。
+
+![fig1](./images/launch_tool.png)
+![fig1](./images/rviz_overview_1.png)
+
+本ツールはROSBAGファイル指定してロードできる他、複数のROSBAGファイルが格納されているディレクトリを指定することも可能です。ただし、その場合には事前に以下のコマンドで`metadata.yaml`の生成が必要になります。
+
+`ros2 bag reindex <DIR_PATH> sqlite3`
+
+![fig1](./images/select_directory.png)
+
+ROSBAGの読み込みが完了したらODDを解析したい時刻を指定します。時刻の指定にはUnix timeを直接指定するほかスライドバーも使用可能です。左に表示されている日時を参考に調整してください。
+
+![fig1](./images/select_time .png)
+
+また、このときViewsのTarget Flameを`base_link`にしておくことで、指定した時刻のEGOの位置と周囲の状況をRvizで可視化可能です。
+
+![fig1](./images/select_target_frame.png)
+
+時刻の指定が完了したら、`Set time stamp`ボタンを押し、最後に`Analyze dynamic ODD factor`を押すことで解析が始まります。
+
+![fig1](./images/rviz_overview_2.png)
+
+```bash
+[rviz2-11] ***********************************************************
+[rviz2-11]                    ODD analysis result
+[rviz2-11] ***********************************************************
+[rviz2-11] Type: TIME SPECIFIED
+[rviz2-11] Time: 2024-04-22 14:48:05
+[rviz2-11]
+[rviz2-11]
+[rviz2-11] - EGO INFO
+[rviz2-11]   [SPEED]                       : 0 [m/s]
+[rviz2-11]   [ELEVATION ANGLE]             : 0.00963597 [rad]
+[rviz2-11]
+[rviz2-11] - EGO BEHAIOVR
+[rviz2-11]   [AVOIDANCE(R)]                : NONE
+[rviz2-11]   [AVOIDANCE(L)]                : NONE
+[rviz2-11]   [LANE_CHANGE(R)]              : NONE
+[rviz2-11]   [LANE_CHANGE(L)]              : NONE
+[rviz2-11]   [START_PLANNER]               : SAFE: true COMMAND: deactivate
+[rviz2-11]   [GOAL_PLANNER]                : NONE
+[rviz2-11]   [CROSSWALK]                   : NONE
+[rviz2-11]   [INTERSECTION]                : NONE
+[rviz2-11]
+[rviz2-11] - LANE INFO
+[rviz2-11]   [ID]                          : 176126
+[rviz2-11]   [WIDTH]                       : 4.24132 [m]
+[rviz2-11]   [SHAPE]                       : STRAIGHT
+[rviz2-11]   [RIGHT LANE NUM]              : 0
+[rviz2-11]   [LEFT LANE NUM]               : 0
+[rviz2-11]   [TOTAL LANE NUM]              : 1
+[rviz2-11]   [SAME DIRECTION LANE]         : NONE
+[rviz2-11]   [OPPOSITE DIRECTION LANE]     : NONE
+[rviz2-11]   [ROAD SHOULDER]               : EXIST
+[rviz2-11]
+[rviz2-11] - SURROUND OBJECT NUM
+[rviz2-11]   [UNKNOWN]                     : 0
+[rviz2-11]   [CAR]                         : 6
+[rviz2-11]   [TRUCK]                       : 0
+[rviz2-11]   [BUS]                         : 3
+[rviz2-11]   [TRAILER]                     : 2
+[rviz2-11]   [MOTORCYCLE]                  : 0
+[rviz2-11]   [BICYCLE]                     : 0
+[rviz2-11]   [PEDESTRIAN]                  : 7
+[rviz2-11] ***********************************************************
+```
+
+## ROSBAG全体に対して経路沿いのODDを解析する場合
 
 現在以下の情報が出力可能です。
 

driving_environment_analyzer/include/driving_environment_analyzer/analyzer_core.hpp

@@ -0,0 +1,101 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef DRIVING_ENVIRONMENT_ANALYZER__ANALYZER_CORE_HPP_
+#define DRIVING_ENVIRONMENT_ANALYZER__ANALYZER_CORE_HPP_
+
+#include "driving_environment_analyzer/type_alias.hpp"
+#include "rosbag2_cpp/reader.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+#include <route_handler/route_handler.hpp>
+
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace driving_environment_analyzer::analyzer_core
+{
+
+struct ODDRawData
+{
+  rcutils_time_point_value_t timestamp;
+  Odometry odometry;
+  PredictedObjects objects;
+  TFMessage tf;
+  TFMessage tf_static;
+  CooperateStatusArray rtc_status;
+};
+
+class AnalyzerCore
+{
+public:
+  explicit AnalyzerCore(rclcpp::Node & node);
+  ~AnalyzerCore();
+
+  bool isDataReadyForStaticODDAnalysis() const;
+  bool isDataReadyForDynamicODDAnalysis() const { return odd_raw_data_.has_value(); }
+
+  void analyzeStaticODDFactor() const;
+  void analyzeDynamicODDFactor() const;
+
+  void setBagFile(const std::string & file_name);
+
+  void setTimeStamp(const rcutils_time_point_value_t & timestamp)
+  {
+    odd_raw_data_ = getRawData(timestamp);
+  }
+
+  void setMap(const HADMapBin & msg) { route_handler_.setMap(msg); }
+
+  void clearData() { odd_raw_data_ = std::nullopt; }
+
+  std::pair<rcutils_time_point_value_t, rcutils_time_point_value_t> getBagStartEndTime()
+  {
+    const auto metadata = reader_.get_metadata();
+    const auto start_time =
+      duration_cast<seconds>(metadata.starting_time.time_since_epoch()).count();
+    const auto duration_time = duration_cast<seconds>(metadata.duration).count();
+    return {start_time, start_time + duration_time};
+  }
+
+  Odometry getOdometry() const { return odd_raw_data_.value().odometry; }
+  PredictedObjects getObjects() const { return odd_raw_data_.value().objects; }
+  TFMessage getTF() const { return odd_raw_data_.value().tf; }
+  TFMessage getTFStatic() const { return odd_raw_data_.value().tf_static; }
+
+private:
+  Pose getEgoPose() const { return odd_raw_data_.value().odometry.pose.pose; }
+
+  double getEgoSpeed() const { return odd_raw_data_.value().odometry.twist.twist.linear.x; }
+
+  template <class T>
+  std::optional<T> getLastTopic(const std::string & topic_name);
+  template <class T>
+  std::optional<T> seekTopic(
+    const std::string & topic_name, const rcutils_time_point_value_t & timestamp);
+  std::optional<ODDRawData> getRawData(const rcutils_time_point_value_t & timestamp);
+
+  std::optional<ODDRawData> odd_raw_data_{std::nullopt};
+
+  route_handler::RouteHandler route_handler_;
+
+  rosbag2_cpp::Reader reader_;
+
+  rclcpp::Logger logger_;
+};
+}  // namespace driving_environment_analyzer::analyzer_core
+
+#endif  // DRIVING_ENVIRONMENT_ANALYZER__ANALYZER_CORE_HPP_

driving_environment_analyzer/include/driving_environment_analyzer/driving_environment_analyzer_node.hpp

@@ -0,0 +1,45 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_NODE_HPP_
+#define DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_NODE_HPP_
+
+#include "driving_environment_analyzer/analyzer_core.hpp"
+#include "driving_environment_analyzer/type_alias.hpp"
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace driving_environment_analyzer
+{
+
+class DrivingEnvironmentAnalyzerNode : public rclcpp::Node
+{
+public:
+  explicit DrivingEnvironmentAnalyzerNode(const rclcpp::NodeOptions & node_options);
+
+private:
+  void onMap(const HADMapBin::ConstSharedPtr map_msg);
+  void analyze();
+
+  std::shared_ptr<analyzer_core::AnalyzerCore> analyzer_;
+
+  rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  rosbag2_cpp::Reader reader_;
+};
+}  // namespace driving_environment_analyzer
+
+#endif  // DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_NODE_HPP_

driving_environment_analyzer/include/driving_environment_analyzer/driving_environment_analyzer_rviz_plugin.hpp

@@ -0,0 +1,84 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_RVIZ_PLUGIN_HPP_
+#define DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_RVIZ_PLUGIN_HPP_
+
+#include "driving_environment_analyzer/analyzer_core.hpp"
+#include "driving_environment_analyzer/type_alias.hpp"
+
+#include <QDir>
+#include <QFileDialog>
+#include <QHBoxLayout>
+#include <QLabel>
+#include <QPushButton>
+#include <QSlider>
+#include <QSpinBox>
+#include <rviz_common/display_context.hpp>
+#include <rviz_common/panel.hpp>
+#include <rviz_common/render_panel.hpp>
+#include <rviz_common/ros_integration/ros_node_abstraction_iface.hpp>
+#include <rviz_common/view_manager.hpp>
+#include <rviz_rendering/render_window.hpp>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace driving_environment_analyzer
+{
+
+class DrivingEnvironmentAnalyzerPanel : public rviz_common::Panel
+{
+  Q_OBJECT
+
+public:
+  explicit DrivingEnvironmentAnalyzerPanel(QWidget * parent = nullptr);
+  ~DrivingEnvironmentAnalyzerPanel() override;
+  void onInitialize() override;
+
+public Q_SLOTS:
+  void onBoxUpdate();
+  void onSliderUpdate();
+  void onClickSetTimeStamp();
+  void onSelectBagFile();
+  void onSelectDirectory();
+  void onClickAnalyzeStaticODDFactor();
+  void onClickAnalyzeDynamicODDFactor();
+
+private:
+  void onMap(const HADMapBin::ConstSharedPtr map_msg);
+
+  std::shared_ptr<analyzer_core::AnalyzerCore> analyzer_;
+
+  QSpinBox * bag_time_selector_;
+  QSlider * bag_time_slider_;
+  QLabel * bag_name_label_;
+  QLabel * bag_time_line_;
+  QPushButton * dir_button_ptr_;
+  QPushButton * file_button_ptr_;
+  QPushButton * analyze_static_odd_button_;
+  QPushButton * analyze_dynamic_odd_button_;
+  QPushButton * set_timestamp_btn_;
+
+  rclcpp::Node::SharedPtr raw_node_;
+  rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
+  rclcpp::Publisher<Odometry>::SharedPtr pub_odometry_;
+  rclcpp::Publisher<PredictedObjects>::SharedPtr pub_objects_;
+  rclcpp::Publisher<TFMessage>::SharedPtr pub_tf_;
+  rclcpp::Publisher<TFMessage>::SharedPtr pub_tf_static_;
+};
+}  // namespace driving_environment_analyzer
+
+#endif  // DRIVING_ENVIRONMENT_ANALYZER__DRIVING_ENVIRONMENT_ANALYZER_RVIZ_PLUGIN_HPP_