Merge pull request autowarefoundation#55 from tier4/sync-upstream

chore: sync upstream
kosuke55 · Jun 8, 2022 · 8de82e6 · 8de82e6
2 parents 8f41322 + d049063
commit 8de82e6
Show file tree

Hide file tree

Showing 94 changed files with 7,703 additions and 197 deletions.
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
@@ -35,7 +35,7 @@ repos:
       - id: yamllint
 
   - repo: https://github.com/tier4/pre-commit-hooks-ros
-    rev: v0.7.0
+    rev: v0.7.1
     hooks:
       - id: flake8-ros
       - id: prettier-xacro
@@ -50,7 +50,7 @@ repos:
       - id: shellcheck
 
   - repo: https://github.com/scop/pre-commit-shfmt
-    rev: v3.4.3-1
+    rev: v3.5.1-1
     hooks:
       - id: shfmt
         args: [-w, -s, -i=4]

diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp
@@ -33,5 +33,6 @@
 #include "tier4_autoware_utils/ros/wait_for_param.hpp"
 #include "tier4_autoware_utils/system/stop_watch.hpp"
 #include "tier4_autoware_utils/trajectory/trajectory.hpp"
+#include "tier4_autoware_utils/vehicle/vehicle_state_checker.hpp"
 
 #endif  // TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_HPP_
diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/vehicle/vehicle_state_checker.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/vehicle/vehicle_state_checker.hpp
@@ -36,7 +36,7 @@ class VehicleStopChecker
 public:
   explicit VehicleStopChecker(rclcpp::Node * node);
 
-  bool isVehicleStopped(const double stop_duration) const;
+  bool isVehicleStopped(const double stop_duration = 0.0) const;
 
   rclcpp::Logger getLogger() { return logger_; }
 
@@ -60,7 +60,7 @@ class VehicleArrivalChecker : public VehicleStopChecker
 public:
   explicit VehicleArrivalChecker(rclcpp::Node * node);
 
-  bool isVehicleStoppedAtStopPoint(const double stop_duration) const;
+  bool isVehicleStoppedAtStopPoint(const double stop_duration = 0.0) const;
 
 private:
   static constexpr double th_arrived_distance_m = 1.0;

diff --git a/common/tier4_autoware_utils/src/vehicle/vehicle_state_checker.cpp b/common/tier4_autoware_utils/src/vehicle/vehicle_state_checker.cpp
@@ -30,7 +30,7 @@ VehicleStopChecker::VehicleStopChecker(rclcpp::Node * node)
     std::bind(&VehicleStopChecker::onOdom, this, _1));
 }
 
-bool VehicleStopChecker::isVehicleStopped(const double stop_duration = 0.0) const
+bool VehicleStopChecker::isVehicleStopped(const double stop_duration) const
 {
   if (twist_buffer_.empty()) {
     return false;
@@ -93,7 +93,7 @@ VehicleArrivalChecker::VehicleArrivalChecker(rclcpp::Node * node) : VehicleStopC
     std::bind(&VehicleArrivalChecker::onTrajectory, this, _1));
 }
 
-bool VehicleArrivalChecker::isVehicleStoppedAtStopPoint(const double stop_duration = 0.0) const
+bool VehicleArrivalChecker::isVehicleStoppedAtStopPoint(const double stop_duration) const
 {
   if (!odometry_ptr_ || !trajectory_ptr_) {
     return false;

diff --git a/common/tier4_autoware_utils/test/src/vehicle/test_vehicle_state_checker.cpp b/common/tier4_autoware_utils/test/src/vehicle/test_vehicle_state_checker.cpp
@@ -142,7 +142,7 @@ TEST(vehicle_stop_checker, isVehicleStopped)
     auto manager = std::make_shared<PubManager>();
     EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected.";
 
-    EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS));
+    EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped());
 
     rclcpp::executors::SingleThreadedExecutor executor;
     executor.add_node(checker);
@@ -153,6 +153,7 @@ TEST(vehicle_stop_checker, isVehicleStopped)
 
     manager->publishStoppedOdometry(ODOMETRY_HISTORY_500_MS);
 
+    EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped());
     EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS));
     EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS));
     EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_400_MS));
@@ -171,7 +172,7 @@ TEST(vehicle_stop_checker, isVehicleStopped)
     auto manager = std::make_shared<PubManager>();
     EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected.";
 
-    EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS));
+    EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped());
 
     rclcpp::executors::SingleThreadedExecutor executor;
     executor.add_node(checker);
@@ -182,6 +183,7 @@ TEST(vehicle_stop_checker, isVehicleStopped)
 
     manager->publishStoppedOdometry(ODOMETRY_HISTORY_1000_MS);
 
+    EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped());
     EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS));
     EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS));
     EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_400_MS));
@@ -200,7 +202,7 @@ TEST(vehicle_stop_checker, isVehicleStopped)
     auto manager = std::make_shared<PubManager>();
     EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected.";
 
-    EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS));
+    EXPECT_FALSE(checker->vehicle_stop_checker_->isVehicleStopped());
 
     rclcpp::executors::SingleThreadedExecutor executor;
     executor.add_node(checker);
@@ -211,6 +213,7 @@ TEST(vehicle_stop_checker, isVehicleStopped)
 
     manager->publishStoppingOdometry(ODOMETRY_HISTORY_1000_MS);
 
+    EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped());
     EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS));
     EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS));
     EXPECT_TRUE(checker->vehicle_stop_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_400_MS));
@@ -232,7 +235,7 @@ TEST(vehicle_arrival_checker, isVehicleStopped)
     auto manager = std::make_shared<PubManager>();
     EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected.";
 
-    EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS));
+    EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStopped());
 
     rclcpp::executors::SingleThreadedExecutor executor;
     executor.add_node(checker);
@@ -243,6 +246,7 @@ TEST(vehicle_arrival_checker, isVehicleStopped)
 
     manager->publishStoppedOdometry(ODOMETRY_HISTORY_500_MS);
 
+    EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStopped());
     EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS));
     EXPECT_TRUE(
       checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS));
@@ -266,7 +270,7 @@ TEST(vehicle_arrival_checker, isVehicleStopped)
     auto manager = std::make_shared<PubManager>();
     EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected.";
 
-    EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS));
+    EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStopped());
 
     rclcpp::executors::SingleThreadedExecutor executor;
     executor.add_node(checker);
@@ -277,6 +281,7 @@ TEST(vehicle_arrival_checker, isVehicleStopped)
 
     manager->publishStoppedOdometry(ODOMETRY_HISTORY_1000_MS);
 
+    EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStopped());
     EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS));
     EXPECT_TRUE(
       checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS));
@@ -300,7 +305,7 @@ TEST(vehicle_arrival_checker, isVehicleStopped)
     auto manager = std::make_shared<PubManager>();
     EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected.";
 
-    EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS));
+    EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStopped());
 
     rclcpp::executors::SingleThreadedExecutor executor;
     executor.add_node(checker);
@@ -311,6 +316,7 @@ TEST(vehicle_arrival_checker, isVehicleStopped)
 
     manager->publishStoppingOdometry(ODOMETRY_HISTORY_1000_MS);
 
+    EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStopped());
     EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS));
     EXPECT_TRUE(
       checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_200_MS));
@@ -337,7 +343,7 @@ TEST(vehicle_arrival_checker, isVehicleStoppedAtStopPoint)
     auto manager = std::make_shared<PubManager>();
     EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected.";
 
-    EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStopped(STOP_DURATION_THRESHOLD_0_MS));
+    EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStopped());
 
     rclcpp::executors::SingleThreadedExecutor executor;
     executor.add_node(checker);
@@ -357,6 +363,7 @@ TEST(vehicle_arrival_checker, isVehicleStoppedAtStopPoint)
     manager->pub_traj_->publish(generateTrajectoryWithStopPoint(goal_pose));
     manager->publishStoppedOdometry(odom_pose, ODOMETRY_HISTORY_500_MS);
 
+    EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint());
     EXPECT_TRUE(
       checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(STOP_DURATION_THRESHOLD_0_MS));
     EXPECT_TRUE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(
@@ -381,8 +388,7 @@ TEST(vehicle_arrival_checker, isVehicleStoppedAtStopPoint)
     auto manager = std::make_shared<PubManager>();
     EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected.";
 
-    EXPECT_FALSE(
-      checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(STOP_DURATION_THRESHOLD_0_MS));
+    EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint());
 
     rclcpp::executors::SingleThreadedExecutor executor;
     executor.add_node(checker);
@@ -402,6 +408,7 @@ TEST(vehicle_arrival_checker, isVehicleStoppedAtStopPoint)
     manager->pub_traj_->publish(generateTrajectoryWithStopPoint(goal_pose));
     manager->publishStoppedOdometry(odom_pose, ODOMETRY_HISTORY_500_MS);
 
+    EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint());
     EXPECT_FALSE(
       checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(STOP_DURATION_THRESHOLD_0_MS));
     EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(
@@ -426,8 +433,7 @@ TEST(vehicle_arrival_checker, isVehicleStoppedAtStopPoint)
     auto manager = std::make_shared<PubManager>();
     EXPECT_GE(manager->pub_odom_->get_subscription_count(), 1U) << "topic is not connected.";
 
-    EXPECT_FALSE(
-      checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(STOP_DURATION_THRESHOLD_0_MS));
+    EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint());
 
     rclcpp::executors::SingleThreadedExecutor executor;
     executor.add_node(checker);
@@ -447,6 +453,7 @@ TEST(vehicle_arrival_checker, isVehicleStoppedAtStopPoint)
     manager->pub_traj_->publish(generateTrajectoryWithoutStopPoint(goal_pose));
     manager->publishStoppedOdometry(odom_pose, ODOMETRY_HISTORY_500_MS);
 
+    EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint());
     EXPECT_FALSE(
       checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(STOP_DURATION_THRESHOLD_0_MS));
     EXPECT_FALSE(checker->vehicle_arrival_checker_->isVehicleStoppedAtStopPoint(

diff --git a/common/tier4_debug_tools/scripts/stop_reason2pose.py b/common/tier4_debug_tools/scripts/stop_reason2pose.py
@@ -93,7 +93,7 @@ def _get_nearest_pose_in_array(self, stop_reason, self_pose):
         if not poses:
             return None
 
-        distances = map(lambda p: StopReason2PoseNode.calc_distance2d(p, self_pose), poses)
+        distances = [StopReason2PoseNode.calc_distance2d(p, self_pose.pose) for p in poses]
         nearest_idx = np.argmin(distances)
 
         return poses[nearest_idx]

diff --git a/common/tier4_traffic_light_rviz_plugin/CMakeLists.txt b/common/tier4_traffic_light_rviz_plugin/CMakeLists.txt
@@ -2,6 +2,23 @@ cmake_minimum_required(VERSION 3.14)
 project(tier4_traffic_light_rviz_plugin)
 
 find_package(autoware_cmake REQUIRED)
+# TODO(Mamoru Sobue): workaround to avoid 'missing tinyxml2::tinyxml2'
+# From tinyxml2_vendor/cmake/Modules/FindTinyXML2.cmake
+find_package(TinyXML2 CONFIG QUIET)
+if(NOT TinyXML2_FOUND)
+  find_path(TINYXML2_INCLUDE_DIR NAMES tinyxml2.h)
+  find_library(TINYXML2_LIBRARY tinyxml2)
+  include(FindPackageHandleStandardArgs)
+  find_package_handle_standard_args(TinyXML2 DEFAULT_MSG TINYXML2_LIBRARY TINYXML2_INCLUDE_DIR)
+  mark_as_advanced(TINYXML2_INCLUDE_DIR TINYXML2_LIBRARY)
+  if(NOT TARGET tinyxml2::tinyxml2)
+    add_library(tinyxml2::tinyxml2 UNKNOWN IMPORTED)
+    set_property(TARGET tinyxml2::tinyxml2 PROPERTY IMPORTED_LOCATION ${TINYXML2_LIBRARY})
+    set_property(TARGET tinyxml2::tinyxml2 PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${TINYXML2_INCLUDE_DIR})
+    list(APPEND TinyXML2_TARGETS tinyxml2::tinyxml2)
+  endif()
+endif()
+
 autoware_package()
 
 find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets)
@@ -18,6 +35,7 @@ target_link_libraries(${PROJECT_NAME}
   ${QT_LIBRARIES}
 )
 
+target_compile_options(${PROJECT_NAME} PUBLIC -Wno-error=deprecated-copy -Wno-error=pedantic)
 # Export the plugin to be imported by rviz2
 pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
 

diff --git a/common/tier4_traffic_light_rviz_plugin/README.md b/common/tier4_traffic_light_rviz_plugin/README.md
@@ -20,6 +20,9 @@ This plugin panel publishes dummy traffic light signals.
 <div align="center">
   <img src="images/select_traffic_light_publish_panel.png" width=50%>
 </div>
+<div align="center">
+  <img src="images/select_traffic_light_id.png" width=50%>
+</div>
 
 1. Start rviz and select panels/Add new panel.
 2. Select TrafficLightPublishPanel and press OK.

diff --git a/common/tier4_traffic_light_rviz_plugin/images/select_traffic_light_id.png b/common/tier4_traffic_light_rviz_plugin/images/select_traffic_light_id.png
diff --git a/common/tier4_traffic_light_rviz_plugin/package.xml b/common/tier4_traffic_light_rviz_plugin/package.xml
@@ -11,7 +11,9 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>autoware_auto_mapping_msgs</depend>
   <depend>autoware_auto_perception_msgs</depend>
+  <depend>lanelet2_extension</depend>
   <depend>libqt5-core</depend>
   <depend>libqt5-gui</depend>
   <depend>libqt5-widgets</depend>

diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp
@@ -22,10 +22,17 @@
 #include <QString>
 #include <QStringList>
 #include <QVBoxLayout>
+#include <lanelet2_extension/regulatory_elements/autoware_traffic_light.hpp>
+#include <lanelet2_extension/utility/message_conversion.hpp>
+#include <lanelet2_extension/utility/query.hpp>
 #include <rviz_common/display_context.hpp>
 
+#include <lanelet2_core/primitives/RegulatoryElement.h>
+
 #include <memory>
 #include <string>
+#include <utility>
+#include <vector>
 
 namespace rviz_plugins
 {
@@ -39,9 +46,8 @@ TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_comm
   publishing_rate_input_->setSuffix("Hz");
 
   // Traffic Light ID
-  traffic_light_id_input_ = new QSpinBox();
-  traffic_light_id_input_->setRange(0, 999999);
-  traffic_light_id_input_->setValue(0);
+  traffic_light_id_input_ = new QComboBox();  // init items in first onVectorMap
+  traffic_light_id_input_->view()->setVerticalScrollBarPolicy(Qt::ScrollBarAsNeeded);
 
   // Traffic Light Confidence
   traffic_light_confidence_input_ = new QDoubleSpinBox();
@@ -122,11 +128,13 @@ TrafficLightPublishPanel::TrafficLightPublishPanel(QWidget * parent) : rviz_comm
   h_layout_5->addWidget(traffic_table_);
 
   setLayout(h_layout_5);
+  received_vector_map_ = false;
 }
 
 void TrafficLightPublishPanel::onSetTrafficLightState()
 {
-  const auto traffic_light_id = traffic_light_id_input_->value();
+  const auto traffic_light_id_str = traffic_light_id_input_->currentText();
+  const auto traffic_light_id = std::stoi(traffic_light_id_str.toStdString());
   const auto color = light_color_combo_->currentText();
   const auto shape = light_shape_combo_->currentText();
   const auto status = light_status_combo_->currentText();
@@ -207,14 +215,19 @@ void TrafficLightPublishPanel::onPublishTrafficLightState()
 
 void TrafficLightPublishPanel::onInitialize()
 {
+  using std::placeholders::_1;
   raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
 
   pub_traffic_signals_ = raw_node_->create_publisher<TrafficSignalArray>(
     "/perception/traffic_light_recognition/traffic_signals", rclcpp::QoS(1));
 
+  sub_vector_map_ = raw_node_->create_subscription<HADMapBin>(
+    "/map/vector_map", rclcpp::QoS{1}.transient_local(),
+    std::bind(&TrafficLightPublishPanel::onVectorMap, this, _1));
   createWallTimer();
 
   enable_publish_ = false;
+  received_vector_map_ = false;
 }
 
 void TrafficLightPublishPanel::onRateChanged(int new_rate)
@@ -350,6 +363,31 @@ void TrafficLightPublishPanel::onTimer()
   }
 }
 
+void TrafficLightPublishPanel::onVectorMap(const HADMapBin::ConstSharedPtr msg)
+{
+  if (received_vector_map_) return;
+  // NOTE: examples from map_loader/lanelet2_map_visualization_node.cpp
+  lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap);
+  lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map);
+  lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map);
+  std::vector<lanelet::TrafficLightConstPtr> tl_reg_elems =
+    lanelet::utils::query::trafficLights(all_lanelets);
+  std::string info = "Fetching traffic lights :";
+  std::string delim = " ";
+  for (auto && tl_reg_elem_ptr : tl_reg_elems) {
+    for (auto && traffic_light : tl_reg_elem_ptr->trafficLights()) {
+      auto id = static_cast<int>(traffic_light.id());
+      info += (std::exchange(delim, ", ") + std::to_string(id));
+      traffic_light_ids_.insert(id);
+    }
+  }
+  RCLCPP_INFO_STREAM(raw_node_->get_logger(), info);
+  received_vector_map_ = true;
+
+  for (auto && traffic_light_id : traffic_light_ids_) {
+    traffic_light_id_input_->addItem(QString::fromStdString(std::to_string(traffic_light_id)));
+  }
+}
 }  // namespace rviz_plugins
 
 #include <pluginlib/class_list_macros.hpp>