[pull] main from autowarefoundation:main

common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp

@@ -19,6 +19,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <visibility_control.hpp>
 
+#include <autoware_auto_perception_msgs/msg/detected_object.hpp>
 #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>
@@ -98,6 +99,11 @@ get_label_marker_ptr(
   const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
   const std::string label, const std_msgs::msg::ColorRGBA & color_rgba);
 
+AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
+get_existence_probability_marker_ptr(
+  const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
+  const float existence_probability, const std_msgs::msg::ColorRGBA & color_rgba);
+
 AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr
 get_uuid_marker_ptr(
   const std::string & uuid, const geometry_msgs::msg::Point & centroid,

common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp

@@ -77,6 +77,11 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     m_display_path_confidence_property{
       "Display Predicted Path Confidence", true, "Enable/disable predicted paths visualization",
       this},
+
+    m_display_existence_probability_property{
+      "Display Existence Probability", false, "Enable/disable existence probability visualization",
+      this},
+
     m_line_width_property{"Line Width", 0.03, "Line width of object-shape", this},
     m_default_topic{default_topic}
   {
@@ -202,6 +207,19 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
       return std::nullopt;
     }
   }
+  template <typename ClassificationContainerT>
+  std::optional<Marker::SharedPtr> get_existence_probability_marker_ptr(
+    const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
+    const float existence_probability, const ClassificationContainerT & labels) const
+  {
+    if (m_display_existence_probability_property.getBool()) {
+      const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels);
+      return detail::get_existence_probability_marker_ptr(
+        centroid, orientation, existence_probability, color_rgba);
+    } else {
+      return std::nullopt;
+    }
+  }
 
   template <typename ClassificationContainerT>
   std::optional<Marker::SharedPtr> get_uuid_marker_ptr(
@@ -326,7 +344,6 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
     }
     return (it->second).label;
   }
-
   std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u) const
   {
     std::stringstream ss;
@@ -415,6 +432,9 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase
   rviz_common::properties::BoolProperty m_display_predicted_paths_property;
   // Property to enable/disable predicted path confidence visualization
   rviz_common::properties::BoolProperty m_display_path_confidence_property;
+
+  rviz_common::properties::BoolProperty m_display_existence_probability_property;
+
   // Property to decide line width of object shape
   rviz_common::properties::FloatProperty m_line_width_property;
   // Default topic name to be visualized

common/autoware_auto_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp

@@ -57,6 +57,22 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg)
       add_marker(label_marker_ptr);
     }
 
+    // Get marker for existence probability
+    geometry_msgs::msg::Point existence_probability_position;
+    existence_probability_position.x = object.kinematics.pose_with_covariance.pose.position.x + 0.5;
+    existence_probability_position.y = object.kinematics.pose_with_covariance.pose.position.y;
+    existence_probability_position.z = object.kinematics.pose_with_covariance.pose.position.z + 0.5;
+    const float existence_probability = object.existence_probability;
+    auto existence_prob_marker = get_existence_probability_marker_ptr(
+      existence_probability_position, object.kinematics.pose_with_covariance.pose.orientation,
+      existence_probability, object.classification);
+    if (existence_prob_marker) {
+      auto existence_prob_marker_ptr = existence_prob_marker.value();
+      existence_prob_marker_ptr->header = msg->header;
+      existence_prob_marker_ptr->id = id++;
+      add_marker(existence_prob_marker_ptr);
+    }
+
     // Get marker for velocity text
     geometry_msgs::msg::Point vel_vis_position;
     vel_vis_position.x = object.kinematics.pose_with_covariance.pose.position.x - 0.5;

common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp

@@ -242,6 +242,23 @@ visualization_msgs::msg::Marker::SharedPtr get_label_marker_ptr(
   return marker_ptr;
 }
 
+visualization_msgs::msg::Marker::SharedPtr get_existence_probability_marker_ptr(
+  const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,
+  const float existence_probability, 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("existence probability");
+  marker_ptr->scale.x = 0.5;
+  marker_ptr->scale.z = 0.5;
+  marker_ptr->text = std::to_string(existence_probability);
+  marker_ptr->action = visualization_msgs::msg::Marker::MODIFY;
+  marker_ptr->pose = marker_ptr->pose = to_pose(centroid, orientation);
+  marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2);
+  marker_ptr->color = color_rgba;
+  return marker_ptr;
+}
+
 visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr(
   const autoware_auto_perception_msgs::msg::Shape & shape_msg,
   const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation,

common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp

@@ -121,6 +121,25 @@ std::vector<visualization_msgs::msg::Marker::SharedPtr> PredictedObjectsDisplay:
       markers.push_back(pose_with_covariance_marker_ptr);
     }
 
+    // Get marker for existence probability
+    geometry_msgs::msg::Point existence_probability_position;
+    existence_probability_position.x =
+      object.kinematics.initial_pose_with_covariance.pose.position.x + 0.5;
+    existence_probability_position.y =
+      object.kinematics.initial_pose_with_covariance.pose.position.y;
+    existence_probability_position.z =
+      object.kinematics.initial_pose_with_covariance.pose.position.z + 0.5;
+    const float existence_probability = object.existence_probability;
+    auto existence_prob_marker = get_existence_probability_marker_ptr(
+      existence_probability_position,
+      object.kinematics.initial_pose_with_covariance.pose.orientation, existence_probability,
+      object.classification);
+    if (existence_prob_marker) {
+      auto existence_prob_marker_ptr = existence_prob_marker.value();
+      existence_prob_marker_ptr->header = msg->header;
+      existence_prob_marker_ptr->id = uuid_to_marker_id(object.object_id);
+      markers.push_back(existence_prob_marker_ptr);
+    }
     // Get marker for velocity text
     geometry_msgs::msg::Point vel_vis_position;
     vel_vis_position.x = uuid_vis_position.x - 0.5;

common/autoware_auto_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp

@@ -102,7 +102,21 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg)
       pose_with_covariance_marker_ptr->id = uuid_to_marker_id(object.object_id);
       add_marker(pose_with_covariance_marker_ptr);
     }
-
+    // Get marker for existence probability
+    geometry_msgs::msg::Point existence_probability_position;
+    existence_probability_position.x = object.kinematics.pose_with_covariance.pose.position.x + 0.5;
+    existence_probability_position.y = object.kinematics.pose_with_covariance.pose.position.y;
+    existence_probability_position.z = object.kinematics.pose_with_covariance.pose.position.z + 0.5;
+    const float existence_probability = object.existence_probability;
+    auto existence_prob_marker = get_existence_probability_marker_ptr(
+      existence_probability_position, object.kinematics.pose_with_covariance.pose.orientation,
+      existence_probability, object.classification);
+    if (existence_prob_marker) {
+      auto existence_prob_marker_ptr = existence_prob_marker.value();
+      existence_prob_marker_ptr->header = msg->header;
+      existence_prob_marker_ptr->id = uuid_to_marker_id(object.object_id);
+      add_marker(existence_prob_marker_ptr);
+    }
     // Get marker for velocity text
     geometry_msgs::msg::Point vel_vis_position;
     vel_vis_position.x = uuid_vis_position.x - 0.5;

common/component_interface_utils/include/component_interface_utils/rclcpp/interface.hpp

@@ -52,7 +52,7 @@ struct NodeInterface
        {ServiceLog::CLIENT_RESPONSE, "client exit"},
        {ServiceLog::ERROR_UNREADY, "client unready"},
        {ServiceLog::ERROR_TIMEOUT, "client timeout"}});
-    RCLCPP_INFO_STREAM(node->get_logger(), type_text.at(type) << ": " << name);
+    RCLCPP_DEBUG_STREAM(node->get_logger(), type_text.at(type) << ": " << name);
 
     ServiceLog msg;
     msg.stamp = node->now();

common/motion_utils/include/motion_utils/trajectory/conversion.hpp

@@ -17,6 +17,7 @@
 
 #include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp"
 #include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp"
+#include "std_msgs/msg/header.hpp"
 
 #include <vector>
 
@@ -34,7 +35,8 @@ namespace motion_utils
  * points larger than the capacity. (Tier IV)
  */
 autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory(
-  const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & trajectory);
+  const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const std_msgs::msg::Header & header = std_msgs::msg::Header{});
 
 /**
  * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to

common/motion_utils/src/resample/resample.cpp

@@ -486,7 +486,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath(
   autoware_auto_planning_msgs::msg::Path resampled_path;
   resampled_path.header = input_path.header;
   resampled_path.left_bound = input_path.left_bound;
-  resampled_path.right_bound = resampled_path.right_bound;
+  resampled_path.right_bound = input_path.right_bound;
   resampled_path.points.resize(interpolated_pose.size());
   for (size_t i = 0; i < resampled_path.points.size(); ++i) {
     autoware_auto_planning_msgs::msg::PathPoint path_point;

common/motion_utils/src/trajectory/conversion.cpp

@@ -30,9 +30,11 @@ namespace motion_utils
  * points larger than the capacity. (Tier IV)
  */
 autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory(
-  const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & trajectory)
+  const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const std_msgs::msg::Header & header)
 {
   autoware_auto_planning_msgs::msg::Trajectory output{};
+  output.header = header;
   for (const auto & pt : trajectory) output.points.push_back(pt);
   return output;
 }

common/tier4_api_utils/include/tier4_api_utils/rclcpp/client.hpp

@@ -41,20 +41,20 @@ class Client
     const typename ServiceT::Request::SharedPtr & request,
     const std::chrono::nanoseconds & timeout = std::chrono::seconds(2))
   {
-    RCLCPP_INFO(logger_, "client request");
+    RCLCPP_DEBUG(logger_, "client request");
 
     if (!client_->service_is_ready()) {
-      RCLCPP_INFO(logger_, "client available");
+      RCLCPP_DEBUG(logger_, "client available");
       return {response_error("Internal service is not available."), nullptr};
     }
 
     auto future = client_->async_send_request(request);
     if (future.wait_for(timeout) != std::future_status::ready) {
-      RCLCPP_INFO(logger_, "client timeout");
+      RCLCPP_DEBUG(logger_, "client timeout");
       return {response_error("Internal service has timed out."), nullptr};
     }
 
-    RCLCPP_INFO(logger_, "client response");
+    RCLCPP_DEBUG(logger_, "client response");
     return {response_success(), future.get()};
   }
 

common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp

@@ -263,9 +263,9 @@ void ManualController::onClickEnableButton()
   {
     auto req = std::make_shared<EngageSrv::Request>();
     req->engage = true;
-    RCLCPP_INFO(raw_node_->get_logger(), "client request");
+    RCLCPP_DEBUG(raw_node_->get_logger(), "client request");
     if (!client_engage_->service_is_ready()) {
-      RCLCPP_INFO(raw_node_->get_logger(), "client is unavailable");
+      RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable");
       return;
     }
     client_engage_->async_send_request(

common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp

@@ -171,15 +171,15 @@ public Q_SLOTS:  // NOLINT for Qt
   {
     auto req = std::make_shared<typename T::Request>();
 
-    RCLCPP_INFO(raw_node_->get_logger(), "client request");
+    RCLCPP_DEBUG(raw_node_->get_logger(), "client request");
 
     if (!client->service_is_ready()) {
-      RCLCPP_INFO(raw_node_->get_logger(), "client is unavailable");
+      RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable");
       return;
     }
 
     client->async_send_request(req, [this](typename rclcpp::Client<T>::SharedFuture result) {
-      RCLCPP_INFO(
+      RCLCPP_DEBUG(
         raw_node_->get_logger(), "Status: %d, %s", result.get()->status.code,
         result.get()->status.message.c_str());
     });

map/map_height_fitter/src/map_height_fitter.cpp

@@ -101,19 +101,19 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point)
   req->area.center_y = point.y;
   req->area.radius = 50;
 
-  RCLCPP_INFO(logger, "Send request to map_loader");
+  RCLCPP_DEBUG(logger, "Send request to map_loader");
   auto future = cli_map_->async_send_request(req);
   auto status = future.wait_for(std::chrono::seconds(1));
   while (status != std::future_status::ready) {
-    RCLCPP_INFO(logger, "waiting response");
+    RCLCPP_DEBUG(logger, "waiting response");
     if (!rclcpp::ok()) {
       return false;
     }
     status = future.wait_for(std::chrono::seconds(1));
   }
 
   const auto res = future.get();
-  RCLCPP_INFO(
+  RCLCPP_DEBUG(
     logger, "Loaded partial pcd map from map_loader (grid size: %lu)",
     res->new_pointcloud_with_ids.size());
 
@@ -168,7 +168,7 @@ std::optional<Point> MapHeightFitter::Impl::fit(const Point & position, const st
   const auto logger = node_->get_logger();
   tf2::Vector3 point(position.x, position.y, position.z);
 
-  RCLCPP_INFO(logger, "original point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ());
+  RCLCPP_DEBUG(logger, "original point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ());
 
   if (cli_map_) {
     if (!get_partial_point_cloud_map(position)) {
@@ -193,7 +193,7 @@ std::optional<Point> MapHeightFitter::Impl::fit(const Point & position, const st
     return std::nullopt;
   }
 
-  RCLCPP_INFO(logger, "modified point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ());
+  RCLCPP_DEBUG(logger, "modified point: %.3f %.3f %.3f", point.getX(), point.getY(), point.getZ());
 
   Point result;
   result.x = point.getX();

planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml

@@ -59,6 +59,10 @@
         max_time_for_object_lat_shift: 0.0 # [s]
         lpf_gain_for_lat_avoid_to_offset: 0.9 # [-]
 
+        max_ego_lat_acc: 0.3        # [m/ss]
+        max_ego_lat_jerk: 0.3       # [m/sss]
+        delay_time_ego_shift: 1.0   # [s]
+
         # for same directional object
         overtaking_object:
           max_time_to_collision: 40.0 # [s]