Merge branch 'autowarefoundation:main' into remove_download_image_pro…

…jection_based_fusion

common/motion_utils/src/resample/resample.cpp

@@ -109,13 +109,16 @@ std::vector<geometry_msgs::msg::Point> resamplePointVector(
 }
 
 std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
-  const std::vector<geometry_msgs::msg::Pose> & points,
+  const std::vector<geometry_msgs::msg::Pose> & points_raw,
   const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy,
   const bool use_lerp_for_z)
 {
+  // Remove overlap points for resampling
+  const auto points = motion_utils::removeOverlapPoints(points_raw);
+
   // validate arguments
   if (!resample_utils::validate_arguments(points, resampled_arclength)) {
-    return points;
+    return points_raw;
   }
 
   std::vector<geometry_msgs::msg::Point> position(points.size());

common/object_recognition_utils/include/object_recognition_utils/transform.hpp

@@ -15,15 +15,22 @@
 #ifndef OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_
 #define OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_
 
+#include <pcl_ros/transforms.hpp>
+
 #include <geometry_msgs/msg/transform.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/point_field.hpp>
 
 #include <boost/optional.hpp>
 
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 #ifdef ROS_DISTRO_GALACTIC
+#include <tf2_eigen/tf2_eigen.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #else
+#include <tf2_eigen/tf2_eigen.hpp>
+
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
@@ -45,6 +52,23 @@ namespace detail
     return boost::none;
   }
 }
+
+[[maybe_unused]] inline boost::optional<Eigen::Matrix4f> getTransformMatrix(
+  const std::string & in_target_frame, const std_msgs::msg::Header & in_cloud_header,
+  const tf2_ros::Buffer & tf_buffer)
+{
+  try {
+    geometry_msgs::msg::TransformStamped transform_stamped;
+    transform_stamped = tf_buffer.lookupTransform(
+      in_target_frame, in_cloud_header.frame_id, in_cloud_header.stamp,
+      rclcpp::Duration::from_seconds(1.0));
+    Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
+    return mat;
+  } catch (tf2::TransformException & e) {
+    RCLCPP_WARN_STREAM(rclcpp::get_logger("detail::getTransformMatrix"), e.what());
+    return boost::none;
+  }
+}
 }  // namespace detail
 
 namespace object_recognition_utils
@@ -79,6 +103,46 @@ bool transformObjects(
   }
   return true;
 }
+template <class T>
+bool transformObjectsWithFeature(
+  const T & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer,
+  T & output_msg)
+{
+  output_msg = input_msg;
+  if (input_msg.header.frame_id != target_frame_id) {
+    output_msg.header.frame_id = target_frame_id;
+    tf2::Transform tf_target2objects_world;
+    tf2::Transform tf_target2objects;
+    tf2::Transform tf_objects_world2objects;
+    const auto ros_target2objects_world = detail::getTransform(
+      tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp);
+    if (!ros_target2objects_world) {
+      return false;
+    }
+    const auto tf_matrix = detail::getTransformMatrix(target_frame_id, input_msg.header, tf_buffer);
+    if (!tf_matrix) {
+      RCLCPP_WARN(
+        rclcpp::get_logger("object_recognition_utils:"), "failed to get transformed matrix");
+      return false;
+    }
+    for (auto & feature_object : output_msg.feature_objects) {
+      // transform object
+      tf2::fromMsg(
+        feature_object.object.kinematics.pose_with_covariance.pose, tf_objects_world2objects);
+      tf_target2objects = tf_target2objects_world * tf_objects_world2objects;
+      tf2::toMsg(tf_target2objects, feature_object.object.kinematics.pose_with_covariance.pose);
+
+      // transform cluster
+      sensor_msgs::msg::PointCloud2 transformed_cluster;
+      pcl_ros::transformPointCloud(*tf_matrix, feature_object.feature.cluster, transformed_cluster);
+      transformed_cluster.header.frame_id = target_frame_id;
+      feature_object.feature.cluster = transformed_cluster;
+    }
+    output_msg.header.frame_id = target_frame_id;
+    return true;
+  }
+  return true;
+}
 }  // namespace object_recognition_utils
 
 #endif  // OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_

common/object_recognition_utils/package.xml

@@ -17,7 +17,13 @@
   <depend>geometry_msgs</depend>
   <depend>interpolation</depend>
   <depend>libboost-dev</depend>
+  <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
   <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
   <depend>tier4_autoware_utils</depend>
 
   <test_depend>ament_cmake_ros</test_depend>

common/perception_utils/package.xml

@@ -4,7 +4,6 @@
   <name>perception_utils</name>
   <version>0.1.0</version>
   <description>The perception_utils package</description>
-  <maintainer email="mingyu.li@tier4.jp">Mingyu Li</maintainer>
   <maintainer email="satoshi.tanaka@tier4.jp">Satoshi Tanaka</maintainer>
   <maintainer email="yusuke.muramatsu@tier4.jp">Yusuke Muramatsu</maintainer>
   <maintainer email="shunsuke.miura@tier4.jp">Shunsuke Miura</maintainer>

common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp

@@ -366,16 +366,28 @@ void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg
   rtc_table_->clearContents();
   num_rtc_status_ptr_->setText(
     QString::fromStdString("The Number of RTC Statuses: " + std::to_string(msg->statuses.size())));
-  if (msg->statuses.empty()) return;
+  if (msg->statuses.empty()) {
+    rtc_table_->update();
+    return;
+  }
   // this is to stable rtc display not to occupy too much
   size_t min_display_size{5};
   size_t max_display_size{10};
   // rtc messages are already sorted by distance
   rtc_table_->setRowCount(
     std::max(min_display_size, std::min(msg->statuses.size(), max_display_size)));
   int cnt = 0;
-  for (auto status : msg->statuses) {
-    if (static_cast<size_t>(cnt) >= max_display_size) return;
+
+  auto sorted_statuses = msg->statuses;
+  std::partition(sorted_statuses.begin(), sorted_statuses.end(), [](const auto & status) {
+    return !status.auto_mode && !uint2bool(status.command_status.type);
+  });
+
+  for (auto status : sorted_statuses) {
+    if (static_cast<size_t>(cnt) >= max_display_size) {
+      rtc_table_->update();
+      return;
+    }
     // uuid
     {
       std::stringstream uuid;

common/tier4_planning_rviz_plugin/include/path/display_base.hpp

@@ -506,6 +506,10 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
       vehicle_footprint_info_ = std::make_shared<VehicleFootprintInfo>(
         vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m,
         vehicle_info_->rear_overhang_m);
+
+      property_vehicle_length_.setValue(vehicle_info_->vehicle_length_m);
+      property_vehicle_width_.setValue(vehicle_info_->vehicle_width_m);
+      property_rear_overhang_.setValue(vehicle_info_->rear_overhang_m);
     } else {
       const float length{property_vehicle_length_.getFloat()};
       const float width{property_vehicle_width_.getFloat()};

common/tier4_system_rviz_plugin/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.14)
+project(tier4_system_rviz_plugin)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+find_package(Qt5 REQUIRED Core Widgets)
+set(QT_LIBRARIES Qt5::Widgets)
+
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+add_definitions(-DQT_NO_KEYWORDS)
+
+set(HEADERS
+  src/mrm_summary_overlay_display.hpp
+  src/jsk_overlay_utils.hpp
+)
+
+## Declare a C++ library
+ament_auto_add_library(tier4_system_rviz_plugin SHARED
+  src/mrm_summary_overlay_display.cpp
+  src/jsk_overlay_utils.cpp
+  ${HEADERS}
+)
+
+target_link_libraries(tier4_system_rviz_plugin
+  ${QT_LIBRARIES}
+)
+
+# Export the plugin to be imported by rviz2
+pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  plugins
+)

common/tier4_system_rviz_plugin/README.md

@@ -0,0 +1 @@
+# tier4_system_rviz_plugin

common/tier4_system_rviz_plugin/package.xml

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>tier4_system_rviz_plugin</name>
+  <version>0.1.0</version>
+  <description>The tier4_vehicle_rviz_plugin package</description>
+  <maintainer email="koji.minoda@tier4.jp">Koji Minoda</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_auto_system_msgs</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>libqt5-core</depend>
+  <depend>libqt5-gui</depend>
+  <depend>libqt5-widgets</depend>
+  <depend>qtbase5-dev</depend>
+  <depend>rviz_common</depend>
+  <depend>rviz_default_plugins</depend>
+  <depend>rviz_ogre_vendor</depend>
+  <depend>tier4_autoware_utils</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

common/tier4_system_rviz_plugin/plugins/plugin_description.xml

@@ -0,0 +1,6 @@
+<library path="tier4_system_rviz_plugin">
+  <class name="rviz_plugins/MrmSummaryOverlayDisplay"
+         type="rviz_plugins::MrmSummaryOverlayDisplay"
+         base_class_type="rviz_common::Display">
+  </class>
+</library>