[pull] main from autowarefoundation:main

.github/workflows/build-and-test-differential.yaml

@@ -74,3 +74,40 @@ jobs:
 
       - name: Show disk space after the tasks
         run: df -h
+
+  clang-tidy-differential:
+    runs-on: ubuntu-latest
+    container: ghcr.io/autowarefoundation/autoware-openadk:latest-prebuilt-cuda
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v3
+        with:
+          fetch-depth: 0
+
+      - name: Show disk space before the tasks
+        run: df -h
+
+      - name: Remove exec_depend
+        uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
+
+      - name: Get modified packages
+        id: get-modified-packages
+        uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1
+
+      - name: Get modified files
+        id: get-modified-files
+        uses: tj-actions/changed-files@v35
+        with:
+          files: |
+            **/*.cpp
+            **/*.hpp
+
+      - name: Run clang-tidy
+        if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }}
+        uses: autowarefoundation/autoware-github-actions/clang-tidy@v1
+        with:
+          rosdistro: humble
+          target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
+          target-files: ${{ steps.get-modified-files.outputs.all_changed_files }}
+          clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy
+          build-depends-repos: build_depends.repos

.github/workflows/pr-agent.yaml

@@ -0,0 +1,31 @@
+name: PR-Agent
+
+on:
+  pull_request:
+    types: [opened, labeled, unlabeled, synchronize]
+  issue_comment:
+
+jobs:
+  prevent-no-label-execution-pr-agent:
+    uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1
+    with:
+      label: tag:pr-agent
+  pr_agent_job:
+    needs: prevent-no-label-execution-pr-agent
+    if: ${{ needs.prevent-no-label-execution-pr-agent.outputs.run == 'true' }}
+    runs-on: ubuntu-latest
+    permissions:
+      issues: write
+      pull-requests: write
+      contents: write
+    name: Run pr agent on every pull request, respond to user comments
+    steps:
+      - name: PR Agent action step
+        id: pragent
+        uses: Codium-ai/pr-agent@main
+        env:
+          OPENAI_KEY: ${{ secrets.OPENAI_KEY_PR_AGENT }}
+          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
+          github_action_config.auto_review: "false"
+          github_action_config.auto_describe: "false"
+          github_action_config.auto_improve: "false"

common/autoware_ad_api_specs/include/autoware_ad_api_specs/vehicle.hpp

@@ -17,9 +17,12 @@
 
 #include <rclcpp/qos.hpp>
 
+#include <autoware_adapi_v1_msgs/msg/door_status_array.hpp>
 #include <autoware_adapi_v1_msgs/msg/vehicle_kinematics.hpp>
 #include <autoware_adapi_v1_msgs/msg/vehicle_status.hpp>
+#include <autoware_adapi_v1_msgs/srv/get_door_layout.hpp>
 #include <autoware_adapi_v1_msgs/srv/get_vehicle_dimensions.hpp>
+#include <autoware_adapi_v1_msgs/srv/set_door_command.hpp>
 
 namespace autoware_ad_api::vehicle
 {
@@ -48,6 +51,27 @@ struct Dimensions
   static constexpr char name[] = "/api/vehicle/dimensions";
 };
 
+struct DoorCommand
+{
+  using Service = autoware_adapi_v1_msgs::srv::SetDoorCommand;
+  static constexpr char name[] = "/api/vehicle/doors/command";
+};
+
+struct DoorLayout
+{
+  using Service = autoware_adapi_v1_msgs::srv::GetDoorLayout;
+  static constexpr char name[] = "/api/vehicle/doors/layout";
+};
+
+struct DoorStatus
+{
+  using Message = autoware_adapi_v1_msgs::msg::DoorStatusArray;
+  static constexpr char name[] = "/api/vehicle/doors/status";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
 }  // namespace autoware_ad_api::vehicle
 
 #endif  // AUTOWARE_AD_API_SPECS__VEHICLE_HPP_

common/component_interface_specs/include/component_interface_specs/vehicle.hpp

@@ -17,6 +17,9 @@
 
 #include <rclcpp/qos.hpp>
 
+#include <autoware_adapi_v1_msgs/msg/door_status_array.hpp>
+#include <autoware_adapi_v1_msgs/srv/get_door_layout.hpp>
+#include <autoware_adapi_v1_msgs/srv/set_door_command.hpp>
 #include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
 #include <autoware_auto_vehicle_msgs/msg/hazard_lights_report.hpp>
 #include <autoware_auto_vehicle_msgs/msg/steering_report.hpp>
@@ -71,6 +74,27 @@ struct EnergyStatus
   static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
 };
 
+struct DoorCommand
+{
+  using Service = autoware_adapi_v1_msgs::srv::SetDoorCommand;
+  static constexpr char name[] = "/vehicle/doors/command";
+};
+
+struct DoorLayout
+{
+  using Service = autoware_adapi_v1_msgs::srv::GetDoorLayout;
+  static constexpr char name[] = "/vehicle/doors/layout";
+};
+
+struct DoorStatus
+{
+  using Message = autoware_adapi_v1_msgs::msg::DoorStatusArray;
+  static constexpr char name[] = "/vehicle/doors/status";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
 }  // namespace vehicle_interface
 
 #endif  // COMPONENT_INTERFACE_SPECS__VEHICLE_HPP_

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -607,8 +607,12 @@ double calcLateralOffset(
     return std::nan("");
   }
 
-  const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx));
-  const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1));
+  const auto p_indices = overlap_removed_points.size() - 2;
+  const auto p_front_idx = (p_indices > seg_idx) ? seg_idx : p_indices;
+  const auto p_back_idx = p_front_idx + 1;
+
+  const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(p_front_idx));
+  const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(p_back_idx));
 
   const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0};
   const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0};

common/object_recognition_utils/include/object_recognition_utils/matching.hpp

@@ -29,6 +29,8 @@
 namespace object_recognition_utils
 {
 using tier4_autoware_utils::Polygon2d;
+// minimum area to avoid division by zero
+static const double MIN_AREA = 1e-6;
 
 inline double getConvexShapeArea(const Polygon2d & source_polygon, const Polygon2d & target_polygon)
 {
@@ -66,10 +68,12 @@ template <class T1, class T2>
 double get2dIoU(const T1 source_object, const T2 target_object, const double min_union_area = 0.01)
 {
   const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object);
+  if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0;
   const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object);
+  if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0;
 
   const double intersection_area = getIntersectionArea(source_polygon, target_polygon);
-  if (intersection_area == 0.0) return 0.0;
+  if (intersection_area < MIN_AREA) return 0.0;
   const double union_area = getUnionArea(source_polygon, target_polygon);
 
   const double iou =
@@ -81,7 +85,9 @@ template <class T1, class T2>
 double get2dGeneralizedIoU(const T1 & source_object, const T2 & target_object)
 {
   const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object);
+  if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0;
   const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object);
+  if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0;
 
   const double intersection_area = getIntersectionArea(source_polygon, target_polygon);
   const double union_area = getUnionArea(source_polygon, target_polygon);
@@ -95,11 +101,13 @@ template <class T1, class T2>
 double get2dPrecision(const T1 source_object, const T2 target_object)
 {
   const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object);
+  const double source_area = boost::geometry::area(source_polygon);
+  if (source_area < MIN_AREA) return 0.0;
   const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object);
+  if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0;
 
   const double intersection_area = getIntersectionArea(source_polygon, target_polygon);
-  if (intersection_area == 0.0) return 0.0;
-  const double source_area = boost::geometry::area(source_polygon);
+  if (intersection_area < MIN_AREA) return 0.0;
 
   return std::min(1.0, intersection_area / source_area);
 }
@@ -108,11 +116,13 @@ template <class T1, class T2>
 double get2dRecall(const T1 source_object, const T2 target_object)
 {
   const auto source_polygon = tier4_autoware_utils::toPolygon2d(source_object);
+  if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0;
   const auto target_polygon = tier4_autoware_utils::toPolygon2d(target_object);
+  const double target_area = boost::geometry::area(target_polygon);
+  if (target_area < MIN_AREA) return 0.0;
 
   const double intersection_area = getIntersectionArea(source_polygon, target_polygon);
-  if (intersection_area == 0.0) return 0.0;
-  const double target_area = boost::geometry::area(target_polygon);
+  if (intersection_area < MIN_AREA) return 0.0;
 
   return std::min(1.0, intersection_area / target_area);
 }

common/tier4_adapi_rviz_plugin/CMakeLists.txt

@@ -12,6 +12,7 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON)
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/route_tool.cpp
   src/route_panel.cpp
+  src/door_panel.cpp
 )
 
 target_link_libraries(${PROJECT_NAME}

common/tier4_adapi_rviz_plugin/plugins/plugin_description.xml

@@ -8,4 +8,8 @@
     <description>RoutePanel</description>
   </class>
 
+  <class type="tier4_adapi_rviz_plugins::DoorPanel" base_class_type="rviz_common::Panel">
+    <description>DoorPanel</description>
+  </class>
+
 </library>

common/tier4_adapi_rviz_plugin/src/door_panel.cpp

@@ -0,0 +1,118 @@
+//  Copyright 2023 The Autoware Contributors
+//
+//  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 "door_panel.hpp"
+
+#include <rviz_common/display_context.hpp>
+
+#include <memory>
+
+namespace tier4_adapi_rviz_plugins
+{
+
+DoorPanel::DoorPanel(QWidget * parent) : rviz_common::Panel(parent)
+{
+  status_ = new QLabel("Trying to get door layout.");
+  layout_ = new QGridLayout();
+  layout_->addWidget(status_, 0, 0, 1, 4);
+  setLayout(layout_);
+}
+
+void DoorPanel::onInitialize()
+{
+  const auto on_layout = [this](const rclcpp::Client<DoorLayout::Service>::SharedFuture future) {
+    const auto res = future.get();
+    if (!res->status.success) {
+      status_->setText(QString::fromStdString("failed to get layout: " + res->status.message));
+      return;
+    }
+    const auto & layouts = res->doors;
+    for (size_t index = 0; index < layouts.size(); ++index) {
+      const auto & layout = layouts[index];
+      DoorUI door;
+      door.description = new QLabel(QString::fromStdString(layout.description));
+      door.status = new QLabel("unknown");
+      door.open = new QPushButton("open");
+      door.close = new QPushButton("close");
+      doors_.push_back(door);
+
+      layout_->addWidget(door.description, index + 1, 0);
+      layout_->addWidget(door.status, index + 1, 1);
+      layout_->addWidget(door.open, index + 1, 2);
+      layout_->addWidget(door.close, index + 1, 3);
+
+      using Command = autoware_adapi_v1_msgs::msg::DoorCommand;
+      const auto on_open = [this, index] { on_button(index, Command::OPEN); };
+      const auto on_close = [this, index] { on_button(index, Command::CLOSE); };
+      connect(door.open, &QPushButton::clicked, on_open);
+      connect(door.close, &QPushButton::clicked, on_close);
+    }
+    status_->hide();
+  };
+
+  const auto on_status = [this](const DoorStatus::Message::ConstSharedPtr msg) {
+    using Status = autoware_adapi_v1_msgs::msg::DoorStatus;
+    if (doors_.size() != msg->doors.size()) {
+      return;
+    }
+    for (size_t index = 0; index < doors_.size(); ++index) {
+      const auto & label = doors_[index].status;
+      switch (msg->doors[index].status) {
+        case Status::NOT_AVAILABLE:
+          label->setText("not available");
+          break;
+        case Status::OPENED:
+          label->setText("opened");
+          break;
+        case Status::CLOSED:
+          label->setText("closed");
+          break;
+        case Status::OPENING:
+          label->setText("opening");
+          break;
+        case Status::CLOSING:
+          label->setText("closing");
+          break;
+        default:
+          label->setText("unknown");
+          break;
+      }
+    }
+  };
+
+  auto lock = getDisplayContext()->getRosNodeAbstraction().lock();
+  auto node = lock->get_raw_node();
+
+  const auto adaptor = component_interface_utils::NodeAdaptor(node.get());
+  adaptor.init_cli(cli_command_);
+  adaptor.init_cli(cli_layout_);
+  adaptor.init_sub(sub_status_, on_status);
+
+  const auto req = std::make_shared<DoorLayout::Service::Request>();
+  cli_layout_->async_send_request(req, on_layout);
+}
+
+void DoorPanel::on_button(uint32_t index, uint8_t command)
+{
+  const auto req = std::make_shared<DoorCommand::Service::Request>();
+  req->doors.resize(1);
+  req->doors.back().index = index;
+  req->doors.back().command = command;
+  cli_command_->async_send_request(req);
+}
+
+}  // namespace tier4_adapi_rviz_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(tier4_adapi_rviz_plugins::DoorPanel, rviz_common::Panel)