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/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> getNextTopic(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,82 @@
+// 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 onClickLoadListFromFile();
+  void onClickAnalyzeStaticODDFactor();
+  void onClickAnalyzeDynamicODDFactor();
+
+private:
+  void onMap(const HADMapBin::ConstSharedPtr map_msg);
+
+  std::shared_ptr<analyzer_core::AnalyzerCore> analyzer_;
+
+  QSpinBox * bag_time_label_;
+  QSlider * bag_time_slider_;
+  QLabel * file_name_label_ptr_;
+  QLabel * bag_time_line_;
+  QPushButton * load_file_btn_ptr_;
+  QPushButton * btn_dynamic_odd_analyze_ptr_;
+  QPushButton * btn_static_odd_analyze_ptr_;
+  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_

driving_environment_analyzer/include/driving_environment_analyzer/type_alias.hpp

@@ -0,0 +1,49 @@
+// 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__TYPE_ALIAS_HPP_
+#define DRIVING_ENVIRONMENT_ANALYZER__TYPE_ALIAS_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <route_handler/route_handler.hpp>
+
+#include "tier4_rtc_msgs/msg/cooperate_status_array.hpp"
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_planning_msgs/msg/lanelet_route.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
+
+namespace driving_environment_analyzer
+{
+
+// std
+using std::chrono::duration_cast;
+using std::chrono::seconds;
+
+// ros2
+using geometry_msgs::msg::Pose;
+using nav_msgs::msg::Odometry;
+using tf2_msgs::msg::TFMessage;
+
+// autoware
+using autoware_auto_mapping_msgs::msg::HADMapBin;
+using autoware_auto_perception_msgs::msg::ObjectClassification;
+using autoware_auto_perception_msgs::msg::PredictedObjects;
+using autoware_planning_msgs::msg::LaneletRoute;
+using tier4_rtc_msgs::msg::CooperateStatusArray;
+
+}  // namespace driving_environment_analyzer
+
+#endif  // DRIVING_ENVIRONMENT_ANALYZER__TYPE_ALIAS_HPP_