Merge pull request #776 from tier4/sync-upstream

chore: sync upstream

common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp

@@ -146,6 +146,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     m_marker_common.addMessage(markers_ptr);
   }
 
+  void deleteMarker(rviz_default_plugins::displays::MarkerID marker_id)
+  {
+    m_marker_common.deleteMarker(marker_id);
+  }
+
 protected:
   /// \brief Convert given shape msg into a Marker
   /// \tparam ClassificationContainerT List type with ObjectClassificationMsg

common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp

@@ -24,6 +24,7 @@
 
 #include <condition_variable>
 #include <list>
+#include <set>
 #include <string>
 #include <unordered_map>
 #include <vector>
@@ -112,6 +113,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay
   std::mutex mutex;
   std::condition_variable condition;
   std::vector<visualization_msgs::msg::Marker::SharedPtr> markers;
+  std::set<rviz_default_plugins::displays::MarkerID> existing_marker_ids;
 };
 
 }  // namespace object_detection

common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp

@@ -15,6 +15,7 @@
 #include <object_detection/predicted_objects_display.hpp>
 
 #include <memory>
+#include <set>
 
 namespace autoware
 {
@@ -130,7 +131,7 @@ std::vector<visualization_msgs::msg::Marker::SharedPtr> PredictedObjectsDisplay:
       auto acceleration_text_marker_ptr = acceleration_text_marker.value();
       acceleration_text_marker_ptr->header = msg->header;
       acceleration_text_marker_ptr->id = uuid_to_marker_id(object.object_id);
-      add_marker(acceleration_text_marker_ptr);
+      markers.push_back(acceleration_text_marker_ptr);
     }
 
     // Get marker for twist
@@ -195,11 +196,19 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt)
   std::unique_lock<std::mutex> lock(mutex);
 
   if (!markers.empty()) {
-    clear_markers();
-
+    std::set new_marker_ids = std::set<rviz_default_plugins::displays::MarkerID>();
     for (const auto & marker : markers) {
+      rviz_default_plugins::displays::MarkerID marker_id =
+        rviz_default_plugins::displays::MarkerID(marker->ns, marker->id);
       add_marker(marker);
+      new_marker_ids.insert(marker_id);
+    }
+    for (auto itr = existing_marker_ids.begin(); itr != existing_marker_ids.end(); itr++) {
+      if (new_marker_ids.find(*itr) == new_marker_ids.end()) {
+        deleteMarker(*itr);
+      }
     }
+    existing_marker_ids = new_marker_ids;
 
     markers.clear();
   }

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/parameter.hpp

@@ -0,0 +1,35 @@
+// Copyright 2023 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 TIER4_AUTOWARE_UTILS__ROS__PARAMETER_HPP_
+#define TIER4_AUTOWARE_UTILS__ROS__PARAMETER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <string>
+
+namespace tier4_autoware_utils
+{
+template <class T>
+T getOrDeclareParameter(rclcpp::Node & node, const std::string & name)
+{
+  if (node.has_parameter(name)) {
+    return node.get_parameter(name).get_value<T>();
+  }
+
+  return node.declare_parameter<T>(name);
+}
+}  // namespace tier4_autoware_utils
+
+#endif  // TIER4_AUTOWARE_UTILS__ROS__PARAMETER_HPP_

common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp

@@ -31,6 +31,7 @@
 #include "tier4_autoware_utils/ros/marker_helper.hpp"
 #include "tier4_autoware_utils/ros/msg_covariance.hpp"
 #include "tier4_autoware_utils/ros/msg_operation.hpp"
+#include "tier4_autoware_utils/ros/parameter.hpp"
 #include "tier4_autoware_utils/ros/processing_time_publisher.hpp"
 #include "tier4_autoware_utils/ros/self_pose_listener.hpp"
 #include "tier4_autoware_utils/ros/transform_listener.hpp"

map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp

@@ -69,8 +69,8 @@ void Lanelet2MapLoaderNode::on_map_projector_info(
   const auto center_line_resolution = get_parameter("center_line_resolution").as_double();
 
   // load map from file
-  const auto map =
-    load_map(lanelet2_filename, msg->type, msg->map_origin.latitude, msg->map_origin.longitude);
+  const auto map = load_map(
+    lanelet2_filename, msg->projector_type, msg->map_origin.latitude, msg->map_origin.longitude);
   if (!map) {
     return;
   }

map/map_projection_loader/README.md

@@ -34,8 +34,9 @@ If you want to use MGRS, please specify the MGRS grid as well.
 
 ```yaml
 # map_projector_info.yaml
-type: "MGRS"
-mgrs_grid: "54SUE"
+projector_type: MGRS
+vertical_datum: WGS84
+mgrs_grid: 54SUE
 ```
 
 ### Using LocalCartesianUTM
@@ -44,10 +45,12 @@ If you want to use local cartesian UTM, please specify the map origin as well.
 
 ```yaml
 # map_projector_info.yaml
-type: "LocalCartesianUTM"
+projector_type: LocalCartesianUTM
+vertical_datum: WGS84
 map_origin:
-  latitude: 35.6092
-  longitude: 139.7303
+  latitude: 35.6762 # [deg]
+  longitude: 139.6503 # [deg]
+  altitude: 0.0 # [m]
 ```
 
 ## Published Topics

map/map_projection_loader/src/load_info_from_lanelet2_map.cpp

@@ -46,10 +46,15 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::str
 
   tier4_map_msgs::msg::MapProjectorInfo msg;
   if (is_local) {
-    msg.type = "local";
+    msg.projector_type = "local";
   } else {
-    msg.type = "MGRS";
+    msg.projector_type = "MGRS";
     msg.mgrs_grid = projector.getProjectedMGRSGrid();
   }
+
+  // We assume that the vertical datum of the map is WGS84 when using lanelet2 map.
+  // However, do note that this is not always true, and may cause problems in the future.
+  // Thus, please consider using the map_projector_info.yaml instead of this deprecated function.
+  msg.vertical_datum = "WGS84";
   return msg;
 }

map/map_projection_loader/src/map_projection_loader.cpp

@@ -27,13 +27,15 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi
   YAML::Node data = YAML::LoadFile(filename);
 
   tier4_map_msgs::msg::MapProjectorInfo msg;
-  msg.type = data["type"].as<std::string>();
-  if (msg.type == "MGRS") {
+  msg.projector_type = data["projector_type"].as<std::string>();
+  if (msg.projector_type == "MGRS") {
+    msg.vertical_datum = data["vertical_datum"].as<std::string>();
     msg.mgrs_grid = data["mgrs_grid"].as<std::string>();
-  } else if (msg.type == "LocalCartesianUTM") {
+  } else if (msg.projector_type == "LocalCartesianUTM") {
+    msg.vertical_datum = data["vertical_datum"].as<std::string>();
     msg.map_origin.latitude = data["map_origin"]["latitude"].as<double>();
     msg.map_origin.longitude = data["map_origin"]["longitude"].as<double>();
-  } else if (msg.type == "local") {
+  } else if (msg.projector_type == "local") {
     ;  // do nothing
   } else {
     throw std::runtime_error(

map/map_projection_loader/test/data/map_projector_info_local.yaml

@@ -0,0 +1 @@
+projector_type: local

map/map_projection_loader/test/data/map_projector_info_local_cartesian_utm.yaml

@@ -0,0 +1,6 @@
+projector_type: LocalCartesianUTM
+vertical_datum: WGS84
+map_origin:
+  latitude: 35.6762 # [deg]
+  longitude: 139.6503 # [deg]
+  altitude: 0.0 # [m]

map/map_projection_loader/test/data/map_projector_info_mgrs.yaml

@@ -0,0 +1,3 @@
+projector_type: MGRS
+vertical_datum: WGS84
+mgrs_grid: 54SUE

map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp

@@ -95,7 +95,7 @@ TEST(TestLoadFromLanelet2Map, LoadMGRSGrid)
   const auto projector_info = load_info_from_lanelet2_map(output_path);
 
   // Check the result
-  EXPECT_EQ(projector_info.type, "MGRS");
+  EXPECT_EQ(projector_info.projector_type, "MGRS");
   EXPECT_EQ(projector_info.mgrs_grid, mgrs_grid);
 }
 
@@ -109,7 +109,7 @@ TEST(TestLoadFromLanelet2Map, LoadLocalGrid)
   const auto projector_info = load_info_from_lanelet2_map(output_path);
 
   // Check the result
-  EXPECT_EQ(projector_info.type, "local");
+  EXPECT_EQ(projector_info.projector_type, "local");
 }
 
 TEST(TestLoadFromLanelet2Map, LoadNoLocalGrid)
@@ -122,7 +122,7 @@ TEST(TestLoadFromLanelet2Map, LoadNoLocalGrid)
   const auto projector_info = load_info_from_lanelet2_map(output_path);
 
   // Check the result
-  EXPECT_EQ(projector_info.type, "MGRS");
+  EXPECT_EQ(projector_info.projector_type, "MGRS");
 }
 
 int main(int argc, char ** argv)

map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py

@@ -33,12 +33,12 @@
 
 logger = get_logger(__name__)
 
-YAML_FILE_PATH = "test/data/projection_info_local_cartesian_utm.yaml"
+YAML_FILE_PATH = "test/data/map_projector_info_local_cartesian_utm.yaml"
 
 
 @pytest.mark.launch_test
 def generate_test_description():
-    map_projection_info_path = os.path.join(
+    map_projector_info_path = os.path.join(
         get_package_share_directory("map_projection_loader"), YAML_FILE_PATH
     )
 
@@ -48,7 +48,7 @@ def generate_test_description():
         output="screen",
         parameters=[
             {
-                "map_projector_info_path": map_projection_info_path,
+                "map_projector_info_path": map_projector_info_path,
                 "lanelet2_map_path": "",
                 "use_local_projector": False,
             },
@@ -121,17 +121,18 @@ def test_node_link(self):
         )
 
         # Load the yaml file directly
-        map_projection_info_path = os.path.join(
+        map_projector_info_path = os.path.join(
             get_package_share_directory("map_projection_loader"), YAML_FILE_PATH
         )
-        with open(map_projection_info_path) as f:
+        with open(map_projector_info_path) as f:
             yaml_data = yaml.load(f, Loader=yaml.FullLoader)
 
         # Test if message received
         self.assertIsNotNone(
             self.received_message, "No message received on map_projector_info topic"
         )
-        self.assertEqual(self.received_message.type, yaml_data["type"])
+        self.assertEqual(self.received_message.projector_type, yaml_data["projector_type"])
+        self.assertEqual(self.received_message.vertical_datum, yaml_data["vertical_datum"])
         self.assertEqual(
             self.received_message.map_origin.latitude, yaml_data["map_origin"]["latitude"]
         )

map/map_projection_loader/test/test_node_load_local_from_yaml.test.py

@@ -33,12 +33,12 @@
 
 logger = get_logger(__name__)
 
-YAML_FILE_PATH = "test/data/projection_info_local.yaml"
+YAML_FILE_PATH = "test/data/map_projector_info_local.yaml"
 
 
 @pytest.mark.launch_test
 def generate_test_description():
-    map_projection_info_path = os.path.join(
+    map_projector_info_path = os.path.join(
         get_package_share_directory("map_projection_loader"), YAML_FILE_PATH
     )
 
@@ -48,7 +48,7 @@ def generate_test_description():
         output="screen",
         parameters=[
             {
-                "map_projector_info_path": map_projection_info_path,
+                "map_projector_info_path": map_projector_info_path,
                 "lanelet2_map_path": "",
                 "use_local_projector": False,
             },
@@ -121,17 +121,17 @@ def test_node_link(self):
         )
 
         # Load the yaml file directly
-        map_projection_info_path = os.path.join(
+        map_projector_info_path = os.path.join(
             get_package_share_directory("map_projection_loader"), YAML_FILE_PATH
         )
-        with open(map_projection_info_path) as f:
+        with open(map_projector_info_path) as f:
             yaml_data = yaml.load(f, Loader=yaml.FullLoader)
 
         # Test if message received
         self.assertIsNotNone(
             self.received_message, "No message received on map_projector_info topic"
         )
-        self.assertEqual(self.received_message.type, yaml_data["type"])
+        self.assertEqual(self.received_message.projector_type, yaml_data["projector_type"])
 
         self.test_node.destroy_subscription(subscription)
 

map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py

@@ -33,12 +33,12 @@
 
 logger = get_logger(__name__)
 
-YAML_FILE_PATH = "test/data/projection_info_mgrs.yaml"
+YAML_FILE_PATH = "test/data/map_projector_info_mgrs.yaml"
 
 
 @pytest.mark.launch_test
 def generate_test_description():
-    map_projection_info_path = os.path.join(
+    map_projector_info_path = os.path.join(
         get_package_share_directory("map_projection_loader"), YAML_FILE_PATH
     )
 
@@ -48,7 +48,7 @@ def generate_test_description():
         output="screen",
         parameters=[
             {
-                "map_projector_info_path": map_projection_info_path,
+                "map_projector_info_path": map_projector_info_path,
                 "lanelet2_map_path": "",
                 "use_local_projector": False,
             },
@@ -121,17 +121,18 @@ def test_node_link(self):
         )
 
         # Load the yaml file directly
-        map_projection_info_path = os.path.join(
+        map_projector_info_path = os.path.join(
             get_package_share_directory("map_projection_loader"), YAML_FILE_PATH
         )
-        with open(map_projection_info_path) as f:
+        with open(map_projector_info_path) as f:
             yaml_data = yaml.load(f, Loader=yaml.FullLoader)
 
         # Test if message received
         self.assertIsNotNone(
             self.received_message, "No message received on map_projector_info topic"
         )
-        self.assertEqual(self.received_message.type, yaml_data["type"])
+        self.assertEqual(self.received_message.projector_type, yaml_data["projector_type"])
+        self.assertEqual(self.received_message.vertical_datum, yaml_data["vertical_datum"])
         self.assertEqual(self.received_message.mgrs_grid, yaml_data["mgrs_grid"])
 
         self.test_node.destroy_subscription(subscription)