chore: sync upstream

.github/CODEOWNERS

@@ -4,7 +4,7 @@
 common/autoware_ad_api_specs/** [email protected]
 common/autoware_auto_common/** [email protected]
 common/autoware_auto_geometry/** [email protected]
-common/autoware_auto_perception_rviz_plugin/** [email protected] taichi.higashide@tier4.jp
+common/autoware_auto_perception_rviz_plugin/** [email protected] [email protected] taiki.tanaka@tier4.jp
 common/autoware_auto_tf2/** [email protected]
 common/autoware_point_types/** [email protected]
 common/autoware_testing/** [email protected]
@@ -25,12 +25,14 @@ common/osqp_interface/** [email protected]
 common/path_distance_calculator/** [email protected]
 common/perception_utils/** [email protected] [email protected] [email protected] [email protected]
 common/polar_grid/** [email protected]
+common/rtc_manager_rviz_plugin/** [email protected]
 common/signal_processing/** [email protected]
 common/tier4_api_utils/** [email protected]
 common/tier4_autoware_utils/** [email protected] [email protected]
 common/tier4_calibration_rviz_plugin/** [email protected]
 common/tier4_control_rviz_plugin/** [email protected]
 common/tier4_datetime_rviz_plugin/** [email protected]
+common/tier4_debug_rviz_plugin/** [email protected]
 common/tier4_debug_tools/** [email protected]
 common/tier4_localization_rviz_plugin/** [email protected]
 common/tier4_perception_rviz_plugin/** [email protected]
@@ -62,7 +64,7 @@ launch/tier4_autoware_api_launch/** [email protected] ryohsuke.mitsudome@tie
 launch/tier4_control_launch/** [email protected]
 launch/tier4_localization_launch/** [email protected]
 launch/tier4_map_launch/** [email protected]
-launch/tier4_perception_launch/** [email protected]
+launch/tier4_perception_launch/** [email protected] [email protected]
 launch/tier4_planning_launch/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 launch/tier4_sensing_launch/** [email protected]
 launch/tier4_simulator_launch/** [email protected]
@@ -94,12 +96,15 @@ perception/ground_segmentation/** [email protected] yukihiro.saito@tier4.
 perception/heatmap_visualizer/** [email protected]
 perception/image_projection_based_fusion/** [email protected] [email protected]
 perception/lidar_apollo_instance_segmentation/** [email protected]
+perception/lidar_apollo_segmentation_tvm/** [email protected]
+perception/lidar_apollo_segmentation_tvm_nodes/** [email protected]
 perception/lidar_centerpoint/** [email protected]
 perception/map_based_prediction/** [email protected]
 perception/multi_object_tracker/** [email protected] [email protected]
 perception/object_merger/** [email protected]
 perception/object_range_splitter/** [email protected]
 perception/occupancy_grid_map_outlier_filter/** [email protected] [email protected]
+perception/probabilistic_occupancy_grid_map/** [email protected]
 perception/radar_fusion_to_detected_object/** [email protected]
 perception/radar_tracks_msgs_converter/** [email protected]
 perception/shape_estimation/** [email protected]
@@ -114,7 +119,7 @@ planning/costmap_generator/** [email protected]
 planning/external_velocity_limit_selector/** [email protected]
 planning/freespace_planner/** [email protected]
 planning/freespace_planning_algorithms/** [email protected]
-planning/mission_planner/** [email protected]
+planning/mission_planner/** [email protected] [email protected]
 planning/motion_velocity_smoother/** [email protected]
 planning/obstacle_avoidance_planner/** [email protected]
 planning/obstacle_cruise_planner/** [email protected] [email protected]
@@ -125,6 +130,7 @@ planning/planning_evaluator/** [email protected]
 planning/route_handler/** [email protected]
 planning/rtc_auto_mode_manager/** [email protected]
 planning/rtc_interface/** [email protected]
+planning/rtc_replayer/** [email protected] [email protected]
 planning/scenario_selector/** [email protected]
 planning/surround_obstacle_checker/** [email protected]
 sensing/geo_pos_conv/** [email protected]
@@ -134,7 +140,6 @@ sensing/image_transport_decompressor/** [email protected]
 sensing/imu_corrector/** [email protected]
 sensing/livox/livox_tag_filter/** [email protected]
 sensing/pointcloud_preprocessor/** [email protected] [email protected]
-sensing/probabilistic_occupancy_grid_map/** [email protected]
 sensing/tier4_pcl_extensions/** [email protected]
 sensing/vehicle_velocity_converter/** [email protected]
 simulator/dummy_perception_publisher/** [email protected]
@@ -155,6 +160,6 @@ system/velodyne_monitor/** [email protected]
 tools/simulator_test/simulator_compatibility_test/** [email protected]
 vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** [email protected]
 vehicle/external_cmd_converter/** [email protected]
-vehicle/raw_vehicle_cmd_converter/** [email protected]
+vehicle/raw_vehicle_cmd_converter/** [email protected] [email protected]
 vehicle/steer_offset_estimator/** [email protected]
 vehicle/vehicle_info_util/** [email protected] [email protected] [email protected] [email protected]

common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp

@@ -22,6 +22,7 @@
 #include <autoware_auto_perception_msgs/msg/object_classification.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_path.hpp>
 #include <autoware_auto_perception_msgs/msg/shape.hpp>
+#include <geometry_msgs/msg/accel.hpp>
 #include <geometry_msgs/msg/pose_with_covariance.hpp>
 #include <geometry_msgs/msg/twist.hpp>
 #include <geometry_msgs/msg/twist_with_covariance.hpp>
@@ -105,6 +106,11 @@ get_velocity_text_marker_ptr(
   const geometry_msgs::msg::Twist & twist, const geometry_msgs::msg::Point & vis_pos,
   const std_msgs::msg::ColorRGBA & color_rgba);
 
+AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
+get_acceleration_text_marker_ptr(
+  const geometry_msgs::msg::Accel & accel, const geometry_msgs::msg::Point & vis_pos,
+  const std_msgs::msg::ColorRGBA & color_rgba);
+
 AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
 get_twist_marker_ptr(
   const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance,

common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp

@@ -68,6 +68,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
       this},
     m_display_velocity_text_property{
       "Display Velocity", true, "Enable/disable velocity text visualization", this},
+    m_display_acceleration_text_property{
+      "Display Acceleration", true, "Enable/disable acceleration text visualization", this},
     m_display_twist_property{"Display Twist", true, "Enable/disable twist visualization", this},
     m_display_predicted_paths_property{
       "Display Predicted Paths", true, "Enable/disable predicted paths visualization", this},
@@ -127,6 +129,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     m_marker_common.addMessage(marker_ptr);
   }
 
+  void add_marker(visualization_msgs::msg::MarkerArray::ConstSharedPtr markers_ptr)
+  {
+    m_marker_common.addMessage(markers_ptr);
+  }
+
 protected:
   /// \brief Convert given shape msg into a Marker
   /// \tparam ClassificationContainerT List type with ObjectClassificationMsg
@@ -207,6 +214,19 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     }
   }
 
+  template <typename ClassificationContainerT>
+  std::optional<Marker::SharedPtr> get_acceleration_text_marker_ptr(
+    const geometry_msgs::msg::Accel & accel, const geometry_msgs::msg::Point & vis_pos,
+    const ClassificationContainerT & labels) const
+  {
+    if (m_display_acceleration_text_property.getBool()) {
+      const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels);
+      return detail::get_acceleration_text_marker_ptr(accel, vis_pos, color_rgba);
+    } else {
+      return std::nullopt;
+    }
+  }
+
   std::optional<Marker::SharedPtr> get_twist_marker_ptr(
     const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance,
     const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) const
@@ -356,6 +376,8 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
   rviz_common::properties::BoolProperty m_display_pose_with_covariance_property;
   // Property to enable/disable velocity text visualization
   rviz_common::properties::BoolProperty m_display_velocity_text_property;
+  // Property to enable/disable acceleration text visualization
+  rviz_common::properties::BoolProperty m_display_acceleration_text_property;
   // Property to enable/disable twist visualization
   rviz_common::properties::BoolProperty m_display_twist_property;
   // Property to enable/disable predicted paths visualization

common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp

@@ -18,12 +18,14 @@
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 
+#include <boost/functional/hash.hpp>
 #include <boost/uuid/uuid.hpp>
 #include <boost/uuid/uuid_generators.hpp>
 
+#include <condition_variable>
 #include <list>
-#include <map>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 namespace autoware
@@ -93,10 +95,23 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay
     return id_map.at(uuid);
   }
 
-  std::map<boost::uuids::uuid, int32_t> id_map;
+  std::vector<visualization_msgs::msg::Marker::SharedPtr> createMarkers(
+    PredictedObjects::ConstSharedPtr msg);
+  void workerThread();
+
+  void update(float wall_dt, float ros_dt) override;
+
+  std::unordered_map<boost::uuids::uuid, int32_t, boost::hash<boost::uuids::uuid>> id_map;
+  // std::unordered_map<boost::uuids::uuid, int32_t> id_map;
   std::list<int32_t> unused_marker_ids;
   int32_t marker_id = 0;
   const int32_t PATH_ID_CONSTANT = 1e3;
+
+  PredictedObjects::ConstSharedPtr msg;
+  bool consumed{false};
+  std::mutex mutex;
+  std::condition_variable condition;
+  std::vector<visualization_msgs::msg::Marker::SharedPtr> markers;
 };
 
 }  // namespace object_detection

common/autoware_auto_perception_rviz_plugin/package.xml

@@ -5,7 +5,8 @@
   <version>1.0.0</version>
   <description>Contains plugins to visualize object detection outputs</description>
   <maintainer email="[email protected]">Apex.AI, Inc.</maintainer>
-  <maintainer email="[email protected]">Taichi Higashide</maintainer>
+  <maintainer email="[email protected]">Satoshi Tanaka</maintainer>
+  <maintainer email="[email protected]">Taiki Tanaka</maintainer>
   <license>Apache 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>

common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp

@@ -20,10 +20,24 @@
 
 #include <algorithm>
 #include <cmath>
+#include <iomanip>
 #include <memory>
+#include <sstream>
 #include <string>
 #include <vector>
 
+namespace
+{
+// get string of double value rounded after first decimal place
+// e.g. roundAfterFirstDecimalPlace(12.345) -> "1.2"
+std::string getRoundedDoubleString(const double val)
+{
+  std::stringstream ss;
+  ss << std::fixed << std::setprecision(1) << val;
+  return ss.str();
+}
+}  // namespace
+
 namespace autoware
 {
 namespace rviz_plugins
@@ -130,6 +144,27 @@ visualization_msgs::msg::Marker::SharedPtr get_velocity_text_marker_ptr(
   return marker_ptr;
 }
 
+visualization_msgs::msg::Marker::SharedPtr get_acceleration_text_marker_ptr(
+  const geometry_msgs::msg::Accel & accel, const geometry_msgs::msg::Point & vis_pos,
+  const std_msgs::msg::ColorRGBA & color_rgba)
+{
+  auto marker_ptr = std::make_shared<Marker>();
+  marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+  marker_ptr->ns = std::string("acceleration");
+  marker_ptr->scale.x = 0.5;
+  marker_ptr->scale.z = 0.5;
+
+  double acc = std::sqrt(
+    accel.linear.x * accel.linear.x + accel.linear.y * accel.linear.y +
+    accel.linear.z * accel.linear.z);
+  marker_ptr->text = getRoundedDoubleString(acc) + std::string("[m/s^2]");
+  marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
+  marker_ptr->pose.position = vis_pos;
+  marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2);
+  marker_ptr->color = color_rgba;
+  return marker_ptr;
+}
+
 visualization_msgs::msg::Marker::SharedPtr get_pose_with_covariance_marker_ptr(
   const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance)
 {