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

chore: sync upstream

.cspell-partial.json

@@ -1,13 +1,5 @@
 {
-  "ignorePaths": [
-    "**/control/**",
-    "**/evaluator/**",
-    "**/launch/**",
-    "**/perception/**",
-    "**/planning/**",
-    "**/sensing/**",
-    "**/system/**"
-  ],
+  "ignorePaths": ["**/control/**", "**/perception/**", "**/planning/**", "**/sensing/**"],
   "ignoreRegExpList": [],
   "words": []
 }

launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml

@@ -53,5 +53,11 @@
         <arg name="input_particle_pose" value="$(var input_pose)"/>
       </include>
     </group>
+
+    <!-- monitor -->
+    <group>
+      <push-ros-namespace namespace="monitor"/>
+      <include file="$(find-pkg-share yabloc_monitor)/launch/yabloc_monitor.launch.xml"/>
+    </group>
   </group>
 </launch>

launch/tier4_localization_launch/package.xml

@@ -26,6 +26,7 @@
   <exec_depend>topic_tools</exec_depend>
   <exec_depend>yabloc_common</exec_depend>
   <exec_depend>yabloc_image_processing</exec_depend>
+  <exec_depend>yabloc_monitor</exec_depend>
   <exec_depend>yabloc_particle_filter</exec_depend>
   <exec_depend>yabloc_pose_initializer</exec_depend>
 

localization/yabloc/yabloc_monitor/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.14)
+project(yabloc_monitor)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_executable(yabloc_monitor
+  src/yabloc_monitor_node.cpp
+  src/yabloc_monitor_core.cpp
+  src/availability_module.cpp
+)
+ament_target_dependencies(yabloc_monitor)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config
+)

localization/yabloc/yabloc_monitor/README.md

@@ -0,0 +1,27 @@
+# yabloc_monitor
+
+YabLoc monitor is a node that monitors the status of the YabLoc localization system. It is a wrapper node that monitors the status of the YabLoc localization system and publishes the status as diagnostics.
+
+## Feature
+
+### Availability
+
+The node monitors the final output pose of YabLoc to verify the availability of YabLoc.
+
+### Others
+
+To be added,
+
+## Interfaces
+
+### Input
+
+| Name                  | Type                        | Description                     |
+| --------------------- | --------------------------- | ------------------------------- |
+| `~/input/yabloc_pose` | `geometry_msgs/PoseStamped` | The final output pose of YabLoc |
+
+### Output
+
+| Name           | Type                              | Description         |
+| -------------- | --------------------------------- | ------------------- |
+| `/diagnostics` | `diagnostic_msgs/DiagnosticArray` | Diagnostics outputs |

localization/yabloc/yabloc_monitor/config/yabloc_monitor.param.yaml

@@ -0,0 +1,3 @@
+/**:
+  ros__parameters:
+    availability/timestamp_tolerance: 1.0

localization/yabloc/yabloc_monitor/launch/yabloc_monitor.launch.xml

@@ -0,0 +1,8 @@
+<launch>
+  <arg name="param_file" default="$(find-pkg-share yabloc_monitor)/config/yabloc_monitor.param.yaml"/>
+
+  <node name="yabloc_monitor" pkg="yabloc_monitor" exec="yabloc_monitor" output="screen">
+    <param from="$(var param_file)"/>
+    <remap from="~/input/yabloc_pose" to="/localization/pose_estimator/yabloc/pf/pose"/>
+  </node>
+</launch>

localization/yabloc/yabloc_monitor/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>yabloc_monitor</name>
+  <version>0.1.0</version>
+  <description>YabLoc monitor package</description>
+  <maintainer email="kento.yabuuchi.2@tier4.jp">Kento Yabuuchi</maintainer>
+  <maintainer email="koji.minoda@tier4.jp">Koji Minoda</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>diagnostic_updater</depend>
+  <depend>geometry_msgs</depend>
+  <depend>rclcpp</depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

localization/yabloc/yabloc_monitor/src/availability_module.cpp

@@ -0,0 +1,47 @@
+// Copyright 2023 TIER IV
+//
+// 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.
+
+#include "availability_module.hpp"
+
+#include <rclcpp/logging.hpp>
+
+#include <memory>
+
+AvailabilityModule::AvailabilityModule(rclcpp::Node * node)
+: clock_(node->get_clock()),
+  latest_yabloc_pose_stamp_(std::nullopt),
+  timestamp_threshold_(node->declare_parameter<double>("availability/timestamp_tolerance"))
+{
+  sub_yabloc_pose_ = node->create_subscription<PoseStamped>(
+    "~/input/yabloc_pose", 10,
+    [this](const PoseStamped::ConstSharedPtr msg) { on_yabloc_pose(msg); });
+}
+
+bool AvailabilityModule::is_available() const
+{
+  if (!latest_yabloc_pose_stamp_.has_value()) {
+    return false;
+  }
+
+  const auto now = clock_->now();
+
+  const auto diff_time = now - latest_yabloc_pose_stamp_.value();
+  const auto diff_time_sec = diff_time.seconds();
+  return diff_time_sec < timestamp_threshold_;
+}
+
+void AvailabilityModule::on_yabloc_pose(const PoseStamped::ConstSharedPtr msg)
+{
+  latest_yabloc_pose_stamp_ = rclcpp::Time(msg->header.stamp);
+}

localization/yabloc/yabloc_monitor/src/availability_module.hpp

@@ -0,0 +1,43 @@
+// Copyright 2023 TIER IV
+//
+// 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 AVAILABILITY_MODULE_HPP_
+#define AVAILABILITY_MODULE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/pose_stamped.hpp>
+
+#include <memory>
+#include <optional>
+
+class AvailabilityModule
+{
+private:
+  using PoseStamped = geometry_msgs::msg::PoseStamped;
+
+public:
+  explicit AvailabilityModule(rclcpp::Node * node);
+  [[nodiscard]] bool is_available() const;
+
+private:
+  rclcpp::Subscription<PoseStamped>::SharedPtr sub_yabloc_pose_;
+  void on_yabloc_pose(PoseStamped::ConstSharedPtr msg);
+
+  rclcpp::Clock::SharedPtr clock_;
+  std::optional<rclcpp::Time> latest_yabloc_pose_stamp_;
+  const double timestamp_threshold_;
+};
+
+#endif  // AVAILABILITY_MODULE_HPP_

localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.cpp

@@ -0,0 +1,48 @@
+// Copyright 2023 TIER IV
+//
+// 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.
+
+#include "yabloc_monitor_core.hpp"
+
+#include <rclcpp/logging.hpp>
+
+#include <memory>
+
+YabLocMonitor::YabLocMonitor() : Node("yabloc_monitor"), updater_(this)
+{
+  updater_.setHardwareID(get_name());
+  updater_.add("yabloc_status", this, &YabLocMonitor::update_diagnostics);
+
+  // Set timer
+  using std::chrono_literals::operator""ms;
+  timer_ = create_wall_timer(100ms, [this] { on_timer(); });
+  // Evaluation modules
+  availability_module_ = std::make_unique<AvailabilityModule>(this);
+}
+
+void YabLocMonitor::on_timer()
+{
+  updater_.force_update();
+}
+
+void YabLocMonitor::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat)
+{
+  bool is_available = availability_module_->is_available();
+  stat.add("Availability", is_available ? "OK" : "NG");
+
+  if (is_available) {
+    stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "OK");
+  } else {
+    stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "NG");
+  }
+}

localization/yabloc/yabloc_monitor/src/yabloc_monitor_core.hpp

@@ -0,0 +1,40 @@
+// Copyright 2023 TIER IV
+//
+// 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 YABLOC_MONITOR_CORE_HPP_
+#define YABLOC_MONITOR_CORE_HPP_
+
+#include "availability_module.hpp"
+
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+
+class YabLocMonitor : public rclcpp::Node
+{
+public:
+  YabLocMonitor();
+
+private:
+  void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  void on_timer();
+
+  diagnostic_updater::Updater updater_;
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  // Evaluation modules
+  std::unique_ptr<AvailabilityModule> availability_module_;
+};
+#endif  // YABLOC_MONITOR_CORE_HPP_

localization/yabloc/yabloc_monitor/src/yabloc_monitor_node.cpp

@@ -0,0 +1,28 @@
+// Copyright 2023 TIER IV
+//
+// 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.
+
+#include "yabloc_monitor_core.hpp"
+
+#include <memory>
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::NodeOptions node_options;
+  auto node = std::make_shared<YabLocMonitor>();
+
+  rclcpp::spin(node);
+
+  return 0;
+}

map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp

@@ -125,13 +125,15 @@ void Lanelet2MapVisualizationNode::onMapBin(
       viz_lanelet_map, "no_obstacle_segmentation_area_for_run_out");
   lanelet::ConstPolygons3d hatched_road_markings_area =
     lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "hatched_road_markings");
+  std::vector<lanelet::NoParkingAreaConstPtr> no_parking_reg_elems =
+    lanelet::utils::query::noParkingAreas(all_lanelets);
 
   std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings,
     cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas,
     cl_speed_bumps, cl_crosswalks, cl_parking_lots, cl_parking_spaces, cl_lanelet_id,
     cl_obstacle_polygons, cl_no_stopping_areas, cl_no_obstacle_segmentation_area,
     cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area,
-    cl_hatched_road_markings_line;
+    cl_hatched_road_markings_line, cl_no_parking_areas;
   setColor(&cl_road, 0.27, 0.27, 0.27, 0.999);
   setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999);
   setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5);
@@ -153,6 +155,7 @@ void Lanelet2MapVisualizationNode::onMapBin(
   setColor(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5);
   setColor(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5);
   setColor(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999);
+  setColor(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5);
 
   visualization_msgs::msg::MarkerArray map_marker_array;
 
@@ -235,6 +238,10 @@ void Lanelet2MapVisualizationNode::onMapBin(
     lanelet::visualization::hatchedRoadMarkingsAreaAsMarkerArray(
       hatched_road_markings_area, cl_hatched_road_markings_area, cl_hatched_road_markings_line));
 
+  insertMarkerArray(
+    &map_marker_array,
+    lanelet::visualization::noParkingAreasAsMarkerArray(no_parking_reg_elems, cl_no_parking_areas));
+
   pub_marker_->publish(map_marker_array);
 }
 

perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp

@@ -99,7 +99,7 @@ void TrafficLightArbiter::onExternalMsg(const TrafficSignalArray::ConstSharedPtr
   if (
     (rclcpp::Time(msg->stamp) - rclcpp::Time(latest_perception_msg_.stamp)).seconds() >
     perception_time_tolerance_) {
-    latest_external_msg_.signals.clear();
+    latest_perception_msg_.signals.clear();
   }
 
   arbitrateAndPublish(msg->stamp);

planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp

@@ -44,6 +44,7 @@ class GoalSearcher : public GoalSearcherBase
   bool checkCollision(const Pose & pose) const;
   bool checkCollisionWithLongitudinalDistance(
     const Pose & ego_pose, const PredictedObjects & dynamic_objects) const;
+  BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
   BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const;
   bool isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const;