Merge pull request #51 from tier4/sync-upstream

chore: sync upstream
tier4 · May 30, 2022 · b4bea5a · b4bea5a
2 parents fe49f69 + 64f636a
commit b4bea5a
Show file tree

Hide file tree

Showing 23 changed files with 418 additions and 291 deletions.
diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml
@@ -31,17 +31,29 @@
 - repository: autowarefoundation/autoware_common
   files:
     - source: .github/workflows/build-and-test.yaml
-      post-commands: 'sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest-cuda" {dest}'
+      post-commands: |
+        sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest-cuda" {dest}
+        sd -f ms '(rosdistro: humble.*?build-depends-repos): build_depends.repos' '$1: build_depends.humble.repos' {dest}
     - source: .github/workflows/build-and-test-differential.yaml
-      post-commands: 'sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest-cuda" {dest}'
+      post-commands: |
+        sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest-cuda" {dest}
+        sd -f ms '(rosdistro: humble.*?build-depends-repos): build_depends.repos' '$1: build_depends.humble.repos' {dest}
     - source: .github/workflows/build-and-test-differential-self-hosted.yaml
-      post-commands: 'sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest-cuda" {dest}'
+      post-commands: |
+        sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest-cuda" {dest}
+        sd -f ms '(rosdistro: humble.*?build-depends-repos): build_depends.repos' '$1: build_depends.humble.repos' {dest}
     - source: .github/workflows/build-and-test-self-hosted.yaml
-      post-commands: 'sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest-cuda" {dest}'
+      post-commands: |
+        sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest-cuda" {dest}
+        sd -f ms '(rosdistro: humble.*?build-depends-repos): build_depends.repos' '$1: build_depends.humble.repos' {dest}
     - source: .github/workflows/check-build-depends.yaml
-      post-commands: 'sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest-cuda" {dest}'
+      post-commands: |
+        sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest-cuda" {dest}
+        sd -f ms '(rosdistro: humble.*?build-depends-repos): build_depends.repos' '$1: build_depends.humble.repos' {dest}
     - source: .github/workflows/clang-tidy-pr-comments.yaml
-      post-commands: 'sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest-cuda" {dest}'
+      post-commands: |
+        sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest-cuda" {dest}
+        sd -f ms '(rosdistro: humble.*?build-depends-repos): build_depends.repos' '$1: build_depends.humble.repos' {dest}
     - source: codecov.yaml
 
 - repository: autowarefoundation/autoware-documentation

diff --git a/.github/workflows/build-and-test-differential-self-hosted.yaml b/.github/workflows/build-and-test-differential-self-hosted.yaml
@@ -18,7 +18,20 @@ jobs:
     needs: prevent-no-label-execution
     if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }}
     runs-on: [self-hosted, linux, ARM64]
-    container: ghcr.io/autowarefoundation/autoware-universe:galactic-latest-cuda
+    container: ${{ matrix.container }}
+    strategy:
+      fail-fast: false
+      matrix:
+        rosdistro:
+          - galactic
+          - humble
+        include:
+          - rosdistro: galactic
+            container: ghcr.io/autowarefoundation/autoware-universe:galactic-latest-cuda
+            build-depends-repos: build_depends.repos
+          - rosdistro: humble
+            container: ghcr.io/autowarefoundation/autoware-universe:humble-latest-cuda
+            build-depends-repos: build_depends.humble.repos
     steps:
       - name: Check out repository
         uses: actions/checkout@v3
@@ -36,14 +49,14 @@ jobs:
         if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }}
         uses: autowarefoundation/autoware-github-actions/colcon-build@v1
         with:
-          rosdistro: galactic
+          rosdistro: ${{ matrix.rosdistro }}
           target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
-          build-depends-repos: build_depends.repos
+          build-depends-repos: ${{ matrix.build-depends-repos }}
 
       - name: Test
         if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }}
         uses: autowarefoundation/autoware-github-actions/colcon-test@v1
         with:
-          rosdistro: galactic
+          rosdistro: ${{ matrix.rosdistro }}
           target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
-          build-depends-repos: build_depends.repos
+          build-depends-repos: ${{ matrix.build-depends-repos }}
diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml
@@ -6,7 +6,20 @@ on:
 jobs:
   build-and-test-differential:
     runs-on: ubuntu-latest
-    container: ghcr.io/autowarefoundation/autoware-universe:galactic-latest-cuda
+    container: ${{ matrix.container }}
+    strategy:
+      fail-fast: false
+      matrix:
+        rosdistro:
+          - galactic
+          - humble
+        include:
+          - rosdistro: galactic
+            container: ghcr.io/autowarefoundation/autoware-universe:galactic-latest-cuda
+            build-depends-repos: build_depends.repos
+          - rosdistro: humble
+            container: ghcr.io/autowarefoundation/autoware-universe:humble-latest-cuda
+            build-depends-repos: build_depends.humble.repos
     steps:
       - name: Cancel previous runs
         uses: styfle/cancel-workflow-action@0.9.1
@@ -27,18 +40,18 @@ jobs:
         if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }}
         uses: autowarefoundation/autoware-github-actions/colcon-build@v1
         with:
-          rosdistro: galactic
+          rosdistro: ${{ matrix.rosdistro }}
           target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
-          build-depends-repos: build_depends.repos
+          build-depends-repos: ${{ matrix.build-depends-repos }}
 
       - name: Test
         id: test
         if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }}
         uses: autowarefoundation/autoware-github-actions/colcon-test@v1
         with:
-          rosdistro: galactic
+          rosdistro: ${{ matrix.rosdistro }}
           target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }}
-          build-depends-repos: build_depends.repos
+          build-depends-repos: ${{ matrix.build-depends-repos }}
 
       - name: Upload coverage to CodeCov
         if: ${{ steps.test.outputs.coverage-report-files != '' }}

diff --git a/.github/workflows/build-and-test-self-hosted.yaml b/.github/workflows/build-and-test-self-hosted.yaml
@@ -8,7 +8,20 @@ on:
 jobs:
   build-and-test-self-hosted:
     runs-on: [self-hosted, linux, ARM64]
-    container: ghcr.io/autowarefoundation/autoware-universe:galactic-latest-cuda
+    container: ${{ matrix.container }}
+    strategy:
+      fail-fast: false
+      matrix:
+        rosdistro:
+          - galactic
+          - humble
+        include:
+          - rosdistro: galactic
+            container: ghcr.io/autowarefoundation/autoware-universe:galactic-latest-cuda
+            build-depends-repos: build_depends.repos
+          - rosdistro: humble
+            container: ghcr.io/autowarefoundation/autoware-universe:humble-latest-cuda
+            build-depends-repos: build_depends.humble.repos
     steps:
       - name: Check out repository
         uses: actions/checkout@v3
@@ -24,14 +37,14 @@ jobs:
         if: ${{ steps.get-self-packages.outputs.self-packages != '' }}
         uses: autowarefoundation/autoware-github-actions/colcon-build@v1
         with:
-          rosdistro: galactic
+          rosdistro: ${{ matrix.rosdistro }}
           target-packages: ${{ steps.get-self-packages.outputs.self-packages }}
-          build-depends-repos: build_depends.repos
+          build-depends-repos: ${{ matrix.build-depends-repos }}
 
       - name: Test
         if: ${{ steps.get-self-packages.outputs.self-packages != '' }}
         uses: autowarefoundation/autoware-github-actions/colcon-test@v1
         with:
-          rosdistro: galactic
+          rosdistro: ${{ matrix.rosdistro }}
           target-packages: ${{ steps.get-self-packages.outputs.self-packages }}
-          build-depends-repos: build_depends.repos
+          build-depends-repos: ${{ matrix.build-depends-repos }}
diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml
@@ -10,7 +10,20 @@ jobs:
   build-and-test:
     if: ${{ github.event_name != 'push' || github.ref_name == github.event.repository.default_branch }}
     runs-on: ubuntu-latest
-    container: ghcr.io/autowarefoundation/autoware-universe:galactic-latest-cuda
+    container: ${{ matrix.container }}
+    strategy:
+      fail-fast: false
+      matrix:
+        rosdistro:
+          - galactic
+          - humble
+        include:
+          - rosdistro: galactic
+            container: ghcr.io/autowarefoundation/autoware-universe:galactic-latest-cuda
+            build-depends-repos: build_depends.repos
+          - rosdistro: humble
+            container: ghcr.io/autowarefoundation/autoware-universe:humble-latest-cuda
+            build-depends-repos: build_depends.humble.repos
     steps:
       - name: Check out repository
         uses: actions/checkout@v3
@@ -26,18 +39,18 @@ jobs:
         if: ${{ steps.get-self-packages.outputs.self-packages != '' }}
         uses: autowarefoundation/autoware-github-actions/colcon-build@v1
         with:
-          rosdistro: galactic
+          rosdistro: ${{ matrix.rosdistro }}
           target-packages: ${{ steps.get-self-packages.outputs.self-packages }}
-          build-depends-repos: build_depends.repos
+          build-depends-repos: ${{ matrix.build-depends-repos }}
 
       - name: Test
         if: ${{ steps.get-self-packages.outputs.self-packages != '' }}
         id: test
         uses: autowarefoundation/autoware-github-actions/colcon-test@v1
         with:
-          rosdistro: galactic
+          rosdistro: ${{ matrix.rosdistro }}
           target-packages: ${{ steps.get-self-packages.outputs.self-packages }}
-          build-depends-repos: build_depends.repos
+          build-depends-repos: ${{ matrix.build-depends-repos }}
 
       - name: Upload coverage to CodeCov
         if: ${{ steps.test.outputs.coverage-report-files != '' }}

diff --git a/build_depends.humble.repos b/build_depends.humble.repos
@@ -0,0 +1,35 @@
+repositories:
+  # core
+  core/autoware_msgs:
+    type: git
+    url: https://github.com/tier4/autoware_auto_msgs.git # TODO(Tier IV): Move to autowarefoundation/autoware_msgs
+    version: tier4/main
+  core/common:
+    type: git
+    url: https://github.com/autowarefoundation/autoware_common.git
+    version: main
+  core/autoware:
+    type: git
+    url: https://github.com/autowarefoundation/autoware.core.git
+    version: main
+  # universe
+  universe/tier4_autoware_msgs:
+    type: git
+    url: https://github.com/tier4/AutowareArchitectureProposal_msgs.git # TODO(Tier IV): Rename to tier4/tier4_autoware_msgs
+    version: tier4/universe
+  universe/vendor/grid_map:
+    type: git
+    url: https://github.com/tier4/grid_map.git
+    version: prepare/humble
+  universe/vendor/navigation2:
+    type: git
+    url: https://github.com/tier4/navigation2.git
+    version: prepare/humble
+  universe/vendor/mussp:
+    type: git
+    url: https://github.com/tier4/muSSP.git
+    version: tier4/main
+  universe/vendor/ndt_omp:
+    type: git
+    url: https://github.com/tier4/ndt_omp.git
+    version: tier4/main
diff --git a/common/tier4_planning_rviz_plugin/include/path_with_lane_id_footprint/display.hpp b/common/tier4_planning_rviz_plugin/include/path_with_lane_id_footprint/display.hpp
@@ -24,6 +24,7 @@
 #include <rviz_common/properties/float_property.hpp>
 #include <rviz_common/properties/parse_color.hpp>
 #include <rviz_common/validate_floats.hpp>
+#include <rviz_rendering/objects/movable_text.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
@@ -35,6 +36,8 @@
 #include <OgreSceneNode.h>
 
 #include <memory>
+#include <utility>
+#include <vector>
 
 namespace rviz_plugins
 {
@@ -67,7 +70,8 @@ private Q_SLOTS:
   rviz_common::properties::FloatProperty * property_vehicle_length_;
   rviz_common::properties::FloatProperty * property_vehicle_width_;
   rviz_common::properties::FloatProperty * property_rear_overhang_;
-
+  rviz_common::properties::BoolProperty * property_lane_id_view_;
+  rviz_common::properties::FloatProperty * property_lane_id_scale_;
   struct VehicleFootprintInfo
   {
     VehicleFootprintInfo(const float l, const float w, const float r)
@@ -79,10 +83,16 @@ private Q_SLOTS:
   std::shared_ptr<VehicleInfo> vehicle_info_;
   std::shared_ptr<VehicleFootprintInfo> vehicle_footprint_info_;
 
+  using LaneIdObject =
+    std::pair<std::unique_ptr<Ogre::SceneNode>, std::unique_ptr<rviz_rendering::MovableText>>;
+  std::vector<LaneIdObject> lane_id_obj_ptrs_;
+
 private:
   autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr last_msg_ptr_;
   bool validateFloats(
     const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr & msg_ptr);
+
+  void allocateLaneIdObjects(const std::size_t size);
 };
 
 }  // namespace rviz_plugins

diff --git a/common/tier4_planning_rviz_plugin/src/path_with_lane_id_footprint/display.cpp b/common/tier4_planning_rviz_plugin/src/path_with_lane_id_footprint/display.cpp
@@ -42,6 +42,11 @@ AutowarePathWithLaneIdFootprintDisplay::AutowarePathWithLaneIdFootprintDisplay()
   property_vehicle_width_->setMin(0.0);
   property_rear_overhang_->setMin(0.0);
 
+  property_lane_id_view_ = new rviz_common::properties::BoolProperty(
+    "View LaneId", true, "", this, SLOT(updateVisualization()), this);
+  property_lane_id_scale_ = new rviz_common::properties::FloatProperty(
+    "Scale", 0.1, "", property_lane_id_view_, SLOT(updateVisualization()), this);
+
   updateVehicleInfo();
 }
 
@@ -65,6 +70,12 @@ void AutowarePathWithLaneIdFootprintDisplay::reset()
 {
   MFDClass::reset();
   path_footprint_manual_object_->clear();
+
+  for (const auto & e : lane_id_obj_ptrs_) {
+    scene_node_->removeChild(e.first.get());
+  }
+  lane_id_obj_ptrs_.clear();
+  lane_id_obj_ptrs_.shrink_to_fit();
 }
 
 bool AutowarePathWithLaneIdFootprintDisplay::validateFloats(
@@ -78,6 +89,28 @@ bool AutowarePathWithLaneIdFootprintDisplay::validateFloats(
   return true;
 }
 
+void AutowarePathWithLaneIdFootprintDisplay::allocateLaneIdObjects(const std::size_t size)
+{
+  if (size > lane_id_obj_ptrs_.size()) {
+    for (std::size_t i = lane_id_obj_ptrs_.size(); i < size; i++) {
+      std::unique_ptr<Ogre::SceneNode> node_ptr;
+      node_ptr.reset(scene_node_->createChildSceneNode());
+      auto text_ptr =
+        std::make_unique<rviz_rendering::MovableText>("not initialized", "Liberation Sans", 0.1);
+      text_ptr->setVisible(false);
+      text_ptr->setTextAlignment(
+        rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE);
+      node_ptr->attachObject(text_ptr.get());
+      lane_id_obj_ptrs_.push_back(std::make_pair(std::move(node_ptr), std::move(text_ptr)));
+    }
+  } else {
+    for (std::size_t i = lane_id_obj_ptrs_.size() - 1; i >= size; i--) {
+      scene_node_->removeChild(lane_id_obj_ptrs_.at(i).first.get());
+    }
+    lane_id_obj_ptrs_.resize(size);
+  }
+}
+
 void AutowarePathWithLaneIdFootprintDisplay::processMessage(
   const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr)
 {
@@ -126,6 +159,8 @@ void AutowarePathWithLaneIdFootprintDisplay::processMessage(
     path_footprint_manual_object_->begin(
       "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST);
 
+    allocateLaneIdObjects(msg_ptr->points.size());
+
     for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) {
       const auto & path_point = msg_ptr->points.at(point_idx);
       /*
@@ -172,6 +207,28 @@ void AutowarePathWithLaneIdFootprintDisplay::processMessage(
           }
         }
       }
+
+      // LaneId
+      if (property_lane_id_view_->getBool()) {
+        Ogre::Vector3 position;
+        position.x = path_point.point.pose.position.x;
+        position.y = path_point.point.pose.position.y;
+        position.z = path_point.point.pose.position.z;
+        auto & node_ptr = lane_id_obj_ptrs_.at(point_idx).first;
+        node_ptr->setPosition(position);
+
+        const auto & text_ptr = lane_id_obj_ptrs_.at(point_idx).second;
+        std::string lane_ids_str = "";
+        for (const auto & e : path_point.lane_ids) {
+          lane_ids_str += std::to_string(e) + ", ";
+        }
+        text_ptr->setCaption(lane_ids_str);
+        text_ptr->setCharacterHeight(property_lane_id_scale_->getFloat());
+        text_ptr->setVisible(true);
+      } else {
+        const auto & text_ptr = lane_id_obj_ptrs_.at(point_idx).second;
+        text_ptr->setVisible(false);
+      }
     }
 
     path_footprint_manual_object_->end();