Merge pull request #862 from tier4/tmp/beta/v0.10.1

feat: update v0.10.1

common/motion_utils/include/motion_utils/marker/marker_helper.hpp

@@ -28,15 +28,18 @@ using geometry_msgs::msg::Pose;
 
 visualization_msgs::msg::MarkerArray createStopVirtualWallMarker(
   const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
-  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");
+  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "",
+  const bool is_driving_forward = true);
 
 visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
   const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
-  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");
+  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "",
+  const bool is_driving_forward = true);
 
 visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
   const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
-  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");
+  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "",
+  const bool is_driving_forward = true);
 
 visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
   const rclcpp::Time & now, const int32_t id);

common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp

@@ -38,6 +38,7 @@ struct VirtualWall
   std::string ns{};
   VirtualWallType style = stop;
   double longitudinal_offset{};
+  bool is_driving_forward{true};
 };
 typedef std::vector<VirtualWall> VirtualWalls;
 
@@ -54,7 +55,7 @@ class VirtualWallMarkerCreator
   using create_wall_function = std::function<visualization_msgs::msg::MarkerArray(
     const geometry_msgs::msg::Pose & pose, const std::string & module_name,
     const rclcpp::Time & now, const int32_t id, const double longitudinal_offset,
-    const std::string & ns_prefix)>;
+    const std::string & ns_prefix, const bool is_driving_forward)>;
 
   VirtualWalls virtual_walls;
   std::unordered_map<std::string, MarkerCount> marker_count_per_namespace;

common/motion_utils/src/marker/marker_helper.cpp

@@ -86,32 +86,35 @@ namespace motion_utils
 {
 visualization_msgs::msg::MarkerArray createStopVirtualWallMarker(
   const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
-  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix)
+  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix,
+  const bool is_driving_forward)
 {
-  const auto pose_with_offset =
-    tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0);
+  const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose(
+    pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0);
   return createVirtualWallMarkerArray(
     pose_with_offset, module_name, ns_prefix + "stop_", now, id,
     createMarkerColor(1.0, 0.0, 0.0, 0.5));
 }
 
 visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
   const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
-  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix)
+  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix,
+  const bool is_driving_forward)
 {
-  const auto pose_with_offset =
-    tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0);
+  const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose(
+    pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0);
   return createVirtualWallMarkerArray(
     pose_with_offset, module_name, ns_prefix + "slow_down_", now, id,
     createMarkerColor(1.0, 1.0, 0.0, 0.5));
 }
 
 visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
   const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
-  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix)
+  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix,
+  const bool is_driving_forward)
 {
-  const auto pose_with_offset =
-    tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0);
+  const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose(
+    pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0);
   return createVirtualWallMarkerArray(
     pose_with_offset, module_name, ns_prefix + "dead_line_", now, id,
     createMarkerColor(0.0, 1.0, 0.0, 0.5));

common/motion_utils/src/marker/virtual_wall_marker_creator.cpp

@@ -64,7 +64,7 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers(
     }
     auto markers = create_fn(
       virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset,
-      virtual_wall.ns);
+      virtual_wall.ns, virtual_wall.is_driving_forward);
     for (auto & marker : markers.markers) {
       marker.id = marker_count_per_namespace[marker.ns].current++;
       marker_array.markers.push_back(marker);

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;

control/operation_mode_transition_manager/src/state.cpp

@@ -121,14 +121,24 @@ bool AutonomousMode::isModeChangeCompleted()
   const auto closest_point = trajectory_.points.at(*closest_idx);
 
   // check for lateral deviation
-  const auto dist_deviation = calcDistance2d(closest_point.pose, kinematics_.pose.pose);
+  const auto dist_deviation =
+    motion_utils::calcLateralOffset(trajectory_.points, kinematics_.pose.pose.position);
+  if (std::isnan(dist_deviation)) {
+    RCLCPP_INFO(logger_, "Not stable yet: lateral offset calculation failed.");
+    return unstable();
+  }
   if (dist_deviation > stable_check_param_.dist_threshold) {
     RCLCPP_INFO(logger_, "Not stable yet: distance deviation is too large: %f", dist_deviation);
     return unstable();
   }
 
   // check for yaw deviation
-  const auto yaw_deviation = calcYawDeviation(closest_point.pose, kinematics_.pose.pose);
+  const auto yaw_deviation =
+    motion_utils::calcYawDeviation(trajectory_.points, kinematics_.pose.pose);
+  if (std::isnan(yaw_deviation)) {
+    RCLCPP_INFO(logger_, "Not stable yet: lateral offset calculation failed.");
+    return unstable();
+  }
   if (yaw_deviation > stable_check_param_.yaw_threshold) {
     RCLCPP_INFO(logger_, "Not stable yet: yaw deviation is too large: %f", yaw_deviation);
     return unstable();

launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml

@@ -11,13 +11,12 @@
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
   <arg name="score_threshold" default="0.35"/>
 
-  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
-  <arg name="use_downsample_pointcloud" default="true" description="use downsample pointcloud in perception"/>
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="use_validator" default="true" description="use obstacle_pointcloud based validator"/>
   <arg name="remove_unknown" default="true"/>
   <arg name="trust_distance" default="30.0"/>
+  <arg name="use_roi_based_cluster" default="false"/>
 
   <!-- Camera param -->
   <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
@@ -59,34 +58,35 @@
     <arg name="image_number" value="$(var image_number)"/>
   </include> -->
 
-  <!-- Pointcloud map filter -->
+  <!-- Pointcloud filter -->
   <group>
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py" if="$(var use_pointcloud_map)">
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py">
       <arg name="input_topic" value="$(var input/obstacle_segmentation/pointcloud)"/>
       <arg name="output_topic" value="pointcloud_map_filtered/pointcloud"/>
       <arg name="use_intra_process" value="true"/>
       <arg name="use_multithread" value="true"/>
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
+      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
     </include>
   </group>
 
   <!-- Clustering -->
   <group>
     <push-ros-namespace namespace="clustering"/>
     <group>
-      <let name="clustering/input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud" if="$(var use_pointcloud_map)"/>
-      <let name="clustering/input/pointcloud" value="$(var input/obstacle_segmentation/pointcloud)" unless="$(var use_pointcloud_map)"/>
+      <let name="euclidean_cluster_output" value="euclidean_cluster/clusters" if="$(var use_roi_based_cluster)"/>
+      <let name="euclidean_cluster_output" value="clusters" unless="$(var use_roi_based_cluster)"/>
       <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
-        <arg name="input_pointcloud" value="$(var clustering/input/pointcloud)"/>
-        <arg name="output_clusters" value="euclidean_cluster/clusters"/>
-        <arg name="use_pointcloud_map" value="false"/>
+        <arg name="input_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
+        <arg name="output_clusters" value="$(var euclidean_cluster_output)"/>
+        <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       </include>
     </group>
 
     <!-- roi based clustering -->
     <group>
-      <include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml">
+      <include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml" if="$(var use_roi_based_cluster)">
         <arg name="input/camera_info0" value="$(var camera_info0)"/>
         <arg name="input/rois0" value="$(var detection_rois0)"/>
         <arg name="input/camera_info1" value="$(var camera_info1)"/>
@@ -119,7 +119,7 @@
 
     <!-- simple_cluster_merger -->
     <group>
-      <include file="$(find-pkg-share cluster_merger)/launch/cluster_merger.launch.xml">
+      <include file="$(find-pkg-share cluster_merger)/launch/cluster_merger.launch.xml" if="$(var use_roi_based_cluster)">
         <arg name="input/cluster0" value="euclidean_cluster/clusters"/>
         <arg name="input/cluster1" value="roi_cluster/clusters"/>
         <arg name="output/clusters" value="clusters"/>

launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml

@@ -11,7 +11,6 @@
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
   <arg name="score_threshold" default="0.35"/>
 
-  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="use_validator" default="true" description="use obstacle_pointcloud based validator"/>
@@ -77,29 +76,29 @@
     </include>
   </group>
 
-  <!-- Pointcloud map filter -->
+  <!-- Pointcloud filter -->
   <group>
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py" if="$(var use_pointcloud_map)">
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py">
       <arg name="input_topic" value="$(var input/obstacle_segmentation/pointcloud)"/>
       <arg name="output_topic" value="pointcloud_map_filtered/pointcloud"/>
       <arg name="use_intra_process" value="true"/>
       <arg name="use_multithread" value="true"/>
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
+      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
     </include>
   </group>
 
   <!-- Clustering -->
   <group>
     <push-ros-namespace namespace="clustering"/>
     <group>
-      <let name="clustering/input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud" if="$(var use_pointcloud_map)"/>
-      <let name="clustering/input/pointcloud" value="$(var input/obstacle_segmentation/pointcloud)" unless="$(var use_pointcloud_map)"/>
+      <let name="euclidean_cluster_output" value="euclidean_cluster/clusters" if="$(var use_roi_based_cluster)"/>
+      <let name="euclidean_cluster_output" value="clusters" unless="$(var use_roi_based_cluster)"/>
       <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
-        <arg name="input_pointcloud" value="$(var clustering/input/pointcloud)"/>
-        <arg name="output_clusters" value="clusters"/>
-        <arg name="use_pointcloud_map" value="false"/>
-        <arg name="use_downsample_pointcloud" value="$(var use_downsample_pointcloud)"/>
+        <arg name="input_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
+        <arg name="output_clusters" value="$(var euclidean_cluster_output)"/>
+        <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       </include>
     </group>
 
@@ -150,7 +149,7 @@
 
       <!-- roi based clustering -->
       <group>
-        <include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml">
+        <include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml" if="$(var use_roi_based_cluster)">
           <arg name="input/camera_info0" value="$(var camera_info0)"/>
           <arg name="input/rois0" value="$(var detection_rois0)"/>
           <arg name="input/camera_info1" value="$(var camera_info1)"/>
@@ -183,7 +182,7 @@
 
       <!-- simple_cluster_merger -->
       <group>
-        <include file="$(find-pkg-share cluster_merger)/launch/cluster_merger.launch.xml">
+        <include file="$(find-pkg-share cluster_merger)/launch/cluster_merger.launch.xml" if="$(var use_roi_based_cluster)">
           <arg name="input/cluster0" value="euclidean_cluster/clusters"/>
           <arg name="input/cluster1" value="roi_cluster/clusters"/>
           <arg name="output/clusters" value="clusters"/>

launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml

@@ -5,7 +5,6 @@
   <!-- lidar param -->
   <arg name="input/pointcloud"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
-  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="container_name" default="pointcloud_container"/>
@@ -65,6 +64,8 @@
       <arg name="remove_unknown" value="$(var remove_unknown)"/>
       <arg name="trust_distance" value="$(var trust_distance)"/>
       <arg name="input/radar" value="$(var input/radar)"/>
+      <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
+      <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
     </include>
   </group>
 
@@ -94,6 +95,8 @@
       <arg name="use_object_filter" value="$(var use_object_filter)"/>
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
+      <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
+      <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
     </include>
   </group>
 
@@ -107,6 +110,7 @@
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
       <arg name="input/radar" value="$(var input/radar)"/>
+      <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
     </include>
   </group>
 
@@ -119,6 +123,7 @@
       <arg name="use_object_filter" value="$(var use_object_filter)"/>
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
+      <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
     </include>
   </group>
 

launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml

@@ -4,35 +4,33 @@
   <arg name="input/obstacle_segmentation/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
   <arg name="output/objects" default="objects"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `clustering`"/>
-  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="container_name" default="pointcloud_container"/>
   <arg name="use_validator" default="true" description="use obstacle_pointcloud based validator"/>
   <arg name="score_threshold" default="0.35"/>
 
-  <!-- Pointcloud map filter -->
+  <!-- Pointcloud filter -->
   <group>
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py" if="$(var use_pointcloud_map)">
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py">
       <arg name="input_topic" value="$(var input/obstacle_segmentation/pointcloud)"/>
       <arg name="output_topic" value="pointcloud_map_filtered/pointcloud"/>
       <arg name="use_intra_process" value="true"/>
       <arg name="use_multithread" value="true"/>
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
+      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
     </include>
   </group>
 
   <!-- Clustering -->
   <group>
     <push-ros-namespace namespace="clustering"/>
-    <let name="clustering/input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud" if="$(var use_pointcloud_map)"/>
-    <let name="clustering/input/pointcloud" value="$(var input/obstacle_segmentation/pointcloud)" unless="$(var use_pointcloud_map)"/>
     <group>
       <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
-        <arg name="input_pointcloud" value="$(var clustering/input/pointcloud)"/>
+        <arg name="input_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
         <arg name="output_clusters" value="clusters"/>
-        <arg name="use_pointcloud_map" value="false"/>
+        <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       </include>
     </group>