Merge pull request #147 from tier4/sync-upstream

chore: sync upstream

.github/CODEOWNERS

@@ -4,7 +4,7 @@
 common/autoware_ad_api_specs/** isamu.takagi@tier4.jp
 common/autoware_auto_common/** opensource@apex.ai
 common/autoware_auto_geometry/** opensource@apex.ai
-common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai taichi.higashide@tier4.jp
+common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp taiki.tanaka@tier4.jp
 common/autoware_auto_tf2/** jit.ray.c@gmail.com
 common/autoware_point_types/** taichi.higashide@tier4.jp
 common/autoware_testing/** adam.dabrowski@robotec.ai
@@ -25,12 +25,14 @@ common/osqp_interface/** maxime.clement@tier4.jp
 common/path_distance_calculator/** isamu.takagi@tier4.jp
 common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp
 common/polar_grid/** yukihiro.saito@tier4.jp
+common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp
 common/signal_processing/** takayuki.murooka@tier4.jp
 common/tier4_api_utils/** isamu.takagi@tier4.jp
 common/tier4_autoware_utils/** kenji.miyake@tier4.jp takamasa.horibe@tier4.jp
 common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp
 common/tier4_control_rviz_plugin/** taiki.tanaka@tier4.jp
 common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp
+common/tier4_debug_rviz_plugin/** takayuki.murooka@tier4.jp
 common/tier4_debug_tools/** kenji.miyake@tier4.jp
 common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp
 common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp
@@ -62,7 +64,7 @@ launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tie
 launch/tier4_control_launch/** takamasa.horibe@tier4.jp
 launch/tier4_localization_launch/** yamato.ando@tier4.jp
 launch/tier4_map_launch/** ryohsuke.mitsudome@tier4.jp
-launch/tier4_perception_launch/** yukihiro.saito@tier4.jp
+launch/tier4_perception_launch/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp
 launch/tier4_planning_launch/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp
 launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp
 launch/tier4_simulator_launch/** keisuke.shima@tier4.jp
@@ -94,12 +96,15 @@ perception/ground_segmentation/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.
 perception/heatmap_visualizer/** kotaro.uetake@tier4.jp
 perception/image_projection_based_fusion/** yukihiro.saito@tier4.jp yusuke.muramatsu@tier4.jp
 perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp
+perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com
+perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com
 perception/lidar_centerpoint/** yusuke.muramatsu@tier4.jp
 perception/map_based_prediction/** yutaka.shimizu@tier4.jp
 perception/multi_object_tracker/** jilada.eccleston@tier4.jp yukihiro.saito@tier4.jp
 perception/object_merger/** yukihiro.saito@tier4.jp
 perception/object_range_splitter/** yukihiro.saito@tier4.jp
 perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp
+perception/probabilistic_occupancy_grid_map/** yukihiro.saito@tier4.jp
 perception/radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp
 perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp
 perception/shape_estimation/** yukihiro.saito@tier4.jp
@@ -114,7 +119,7 @@ planning/costmap_generator/** kenji.miyake@tier4.jp
 planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp
 planning/freespace_planner/** takamasa.horibe@tier4.jp
 planning/freespace_planning_algorithms/** takamasa.horibe@tier4.jp
-planning/mission_planner/** ryohsuke.mitsudome@tier4.jp
+planning/mission_planner/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp
 planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp
 planning/obstacle_avoidance_planner/** takayuki.murooka@tier4.jp
 planning/obstacle_cruise_planner/** takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp
@@ -125,6 +130,7 @@ planning/planning_evaluator/** maxime.clement@tier4.jp
 planning/route_handler/** fumiya.watanabe@tier4.jp
 planning/rtc_auto_mode_manager/** fumiya.watanabe@tier4.jp
 planning/rtc_interface/** fumiya.watanabe@tier4.jp
+planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp
 planning/scenario_selector/** kenji.miyake@tier4.jp
 planning/surround_obstacle_checker/** satoshi.ota@tier4.jp
 sensing/geo_pos_conv/** yamato.ando@tier4.jp
@@ -134,7 +140,6 @@ sensing/image_transport_decompressor/** yukihiro.saito@tier4.jp
 sensing/imu_corrector/** yamato.ando@tier4.jp
 sensing/livox/livox_tag_filter/** kenji.miyake@tier4.jp
 sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com yukihiro.saito@tier4.jp
-sensing/probabilistic_occupancy_grid_map/** yukihiro.saito@tier4.jp
 sensing/tier4_pcl_extensions/** ryu.yamamoto@tier4.jp
 sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp
 simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp
@@ -155,6 +160,6 @@ system/velodyne_monitor/** fumihito.ito@tier4.jp
 tools/simulator_test/simulator_compatibility_test/** shpark@morai.ai
 vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/** tomoya.kimura@tier4.jp
 vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp
-vehicle/raw_vehicle_cmd_converter/** takamasa.horibe@tier4.jp
+vehicle/raw_vehicle_cmd_converter/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp
 vehicle/steer_offset_estimator/** taiki.tanaka@tier4.jp
 vehicle/vehicle_info_util/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp yamato.ando@tier4.jp

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="opensource@apex.ai">Apex.AI, Inc.</maintainer>
-  <maintainer email="taichi.higashide@tier4.jp">Taichi Higashide</maintainer>
+  <maintainer email="satoshi.tanaka@tier4.jp">Satoshi Tanaka</maintainer>
+  <maintainer email="taiki.tanaka@tier4.jp">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)
 {