Merge branch 'main' into feat/lane_change_rtc_state

common/motion_utils/include/motion_utils/factor/velocity_factor_interface.hpp

@@ -19,7 +19,6 @@
 #include <autoware_adapi_v1_msgs/msg/planning_behavior.hpp>
 #include <autoware_adapi_v1_msgs/msg/velocity_factor.hpp>
 #include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
-#include <autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 
 #include <string>
@@ -40,10 +39,10 @@ class VelocityFactorInterface
   void init(const VelocityFactorBehavior & behavior) { behavior_ = behavior; }
   void reset() { velocity_factor_.behavior = PlanningBehavior::UNKNOWN; }
 
+  template <class PointType>
   void set(
-    const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
-    const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status,
-    const std::string & detail = "");
+    const std::vector<PointType> & points, const Pose & curr_pose, const Pose & stop_pose,
+    const VelocityFactorStatus status, const std::string & detail = "");
 
 private:
   VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN};

common/motion_utils/src/factor/velocity_factor_interface.cpp

@@ -15,12 +15,16 @@
 #include <motion_utils/factor/velocity_factor_interface.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
 
+#include <autoware_auto_planning_msgs/msg/path_point.hpp>
+#include <autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp>
+#include <autoware_auto_planning_msgs/msg/trajectory_point.hpp>
+
 namespace motion_utils
 {
+template <class PointType>
 void VelocityFactorInterface::set(
-  const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
-  const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status,
-  const std::string & detail)
+  const std::vector<PointType> & points, const Pose & curr_pose, const Pose & stop_pose,
+  const VelocityFactorStatus status, const std::string & detail)
 {
   const auto & curr_point = curr_pose.position;
   const auto & stop_point = stop_pose.position;
@@ -32,4 +36,14 @@ void VelocityFactorInterface::set(
   velocity_factor_.detail = detail;
 }
 
+template void VelocityFactorInterface::set<autoware_auto_planning_msgs::msg::PathPointWithLaneId>(
+  const std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> &, const Pose &,
+  const Pose &, const VelocityFactorStatus, const std::string &);
+template void VelocityFactorInterface::set<autoware_auto_planning_msgs::msg::PathPoint>(
+  const std::vector<autoware_auto_planning_msgs::msg::PathPoint> &, const Pose &, const Pose &,
+  const VelocityFactorStatus, const std::string &);
+template void VelocityFactorInterface::set<autoware_auto_planning_msgs::msg::TrajectoryPoint>(
+  const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> &, const Pose &,
+  const Pose &, const VelocityFactorStatus, const std::string &);
+
 }  // namespace motion_utils

common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp

@@ -316,6 +316,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
         node->detachAllObjects();
         node->removeAndDestroyAllChildren();
         this->scene_manager_->destroySceneNode(node);
+
+        rviz_rendering::MovableText * text = velocity_texts_.at(i);
+        delete text;
       }
       velocity_texts_.resize(msg_ptr->points.size());
       velocity_text_nodes_.resize(msg_ptr->points.size());
@@ -339,6 +342,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay<T>
         node->detachAllObjects();
         node->removeAndDestroyAllChildren();
         this->scene_manager_->destroySceneNode(node);
+
+        rviz_rendering::MovableText * text = slope_texts_.at(i);
+        delete text;
       }
       slope_texts_.resize(msg_ptr->points.size());
       slope_text_nodes_.resize(msg_ptr->points.size());

control/autonomous_emergency_braking/src/node.cpp

@@ -301,10 +301,13 @@ bool AEB::fetchLatestData()
   }
 
   const auto imu_ptr = sub_imu_.takeData();
-  if (use_imu_path_ && !imu_ptr) {
-    return missing("imu message");
+  if (use_imu_path_) {
+    if (!imu_ptr) {
+      return missing("imu message");
+    }
+    // imu_ptr is valid
+    onImu(imu_ptr);
   }
-  onImu(imu_ptr);
   if (use_imu_path_ && !angular_velocity_ptr_) {
     return missing("imu");
   }

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml

@@ -2,6 +2,24 @@
   <arg name="interface_input_topic" default="/planning/scenario_planning/lane_driving/behavior_planning/path"/>
   <arg name="interface_output_topic" default="/planning/scenario_planning/lane_driving/trajectory"/>
 
+  <arg name="launch_motion_out_of_lane_module" default="true"/>
+  <arg name="launch_dynamic_obstacle_stop_module" default="true"/>
+  <arg name="launch_module_list_end" default="&quot;&quot;]"/>
+
+  <!-- assemble launch config for motion velocity planner -->
+  <arg name="motion_velocity_planner_launch_modules" default="["/>
+  <let
+    name="motion_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var motion_velocity_planner_launch_modules)' + 'autoware::motion_velocity_planner::OutOfLaneModule, '&quot;)"
+    if="$(var launch_motion_out_of_lane_module)"
+  />
+  <let
+    name="motion_velocity_planner_launch_modules"
+    value="$(eval &quot;'$(var motion_velocity_planner_launch_modules)' + 'motion_velocity_planner::DynamicObstacleStopModule, '&quot;)"
+    if="$(var launch_dynamic_obstacle_stop_module)"
+  />
+  <let name="motion_velocity_planner_launch_modules" value="$(eval &quot;'$(var motion_velocity_planner_launch_modules)' + '$(var launch_module_list_end)'&quot;)"/>
+
   <node_container pkg="rclcpp_components" exec="$(var container_type)" name="motion_planning_container" namespace="" args="" output="screen">
     <composable_node pkg="glog_component" plugin="GlogComponent" name="glog_component" namespace=""/>
   </node_container>
@@ -91,12 +109,44 @@
     </group>
   </group>
 
-  <!-- velocity limiter -->
+  <!-- plan slowdown or stops on the final trajectory -->
   <group>
     <load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
-      <composable_node pkg="obstacle_velocity_limiter" plugin="obstacle_velocity_limiter::ObstacleVelocityLimiterNode" name="obstacle_velocity_limiter" namespace="">
+      <composable_node pkg="autoware_motion_velocity_planner_node" plugin="autoware::motion_velocity_planner::MotionVelocityPlannerNode" name="motion_velocity_planner" namespace="">
         <!-- topic remap -->
         <remap from="~/input/trajectory" to="obstacle_avoidance_planner/trajectory"/>
+        <remap from="~/input/vector_map" to="/map/vector_map"/>
+        <remap from="~/input/vehicle_odometry" to="/localization/kinematic_state"/>
+        <remap from="~/input/accel" to="/localization/acceleration"/>
+        <remap from="~/input/dynamic_objects" to="/perception/object_recognition/objects"/>
+        <remap from="~/input/no_ground_pointcloud" to="/perception/obstacle_segmentation/pointcloud"/>
+        <remap from="~/input/traffic_signals" to="/perception/traffic_light_recognition/traffic_signals"/>
+        <remap from="~/input/virtual_traffic_light_states" to="/perception/virtual_traffic_light_states"/>
+        <remap from="~/input/occupancy_grid" to="/perception/occupancy_grid_map/map"/>
+        <remap from="~/output/trajectory" to="motion_velocity_planner/trajectory"/>
+        <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
+        <remap from="~/output/velocity_factors" to="/planning/velocity_factors/motion_velocity_planner"/>
+        <!-- params -->
+        <param name="launch_modules" value="$(var motion_velocity_planner_launch_modules)"/>
+        <param from="$(var common_param_path)"/>
+        <param from="$(var vehicle_param_file)"/>
+        <param from="$(var nearest_search_param_path)"/>
+        <param from="$(var motion_velocity_smoother_param_path)"/>
+        <param from="$(var motion_velocity_smoother_type_param_path)"/>
+        <param from="$(var motion_velocity_planner_param_path)"/>
+        <param from="$(var motion_velocity_planner_out_of_lane_module_param_path)"/>
+        <!-- <param from="$(var motion_velocity_planner_template_param_path)"/> -->
+        <!-- composable node config -->
+        <extra_arg name="use_intra_process_comms" value="false"/>
+      </composable_node>
+    </load_composable_node>
+  </group>
+  <!-- velocity limiter TODO(Maxime): move this node to the motion_velocity_planner -->
+  <group>
+    <load_composable_node target="/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container">
+      <composable_node pkg="obstacle_velocity_limiter" plugin="obstacle_velocity_limiter::ObstacleVelocityLimiterNode" name="obstacle_velocity_limiter" namespace="">
+        <!-- topic remap -->
+        <remap from="~/input/trajectory" to="motion_velocity_planner/trajectory"/>
         <remap from="~/input/odometry" to="/localization/kinematic_state"/>
         <remap from="~/input/dynamic_obstacles" to="/perception/object_recognition/objects"/>
         <remap from="~/input/occupancy_grid" to="/perception/occupancy_grid_map/map"/>

perception/detected_object_validation/config/object_lanelet_filter.param.yaml

@@ -9,3 +9,13 @@
       MOTORCYCLE : false
       BICYCLE : false
       PEDESTRIAN : false
+
+    filter_settings:
+      # polygon overlap based filter
+      polygon_overlap_filter:
+       enabled: true
+      # velocity direction based filter
+      lanelet_direction_filter:
+        enabled: false
+        velocity_yaw_threshold: 0.785398 # [rad] (45 deg)
+        object_speed_threshold: 3.0 # [m/s]

perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp

@@ -17,6 +17,7 @@
 
 #include "detected_object_validation/utils/utils.hpp"
 
+#include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/ros/debug_publisher.hpp>
@@ -62,11 +63,27 @@ class ObjectLaneletFilterNode : public rclcpp::Node
   tf2_ros::TransformListener tf_listener_;
 
   utils::FilterTargetLabel filter_target_;
-
+  struct FilterSettings
+  {
+    bool polygon_overlap_filter;
+    bool lanelet_direction_filter;
+    double lanelet_direction_filter_velocity_yaw_threshold;
+    double lanelet_direction_filter_object_speed_threshold;
+  } filter_settings_;
+
+  bool filterObject(
+    const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object,
+    const autoware_auto_perception_msgs::msg::DetectedObject & input_object,
+    const lanelet::ConstLanelets & intersected_road_lanelets,
+    const lanelet::ConstLanelets & intersected_shoulder_lanelets,
+    autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg);
   LinearRing2d getConvexHull(const autoware_auto_perception_msgs::msg::DetectedObjects &);
   lanelet::ConstLanelets getIntersectedLanelets(
     const LinearRing2d &, const lanelet::ConstLanelets &);
   bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &);
+  bool isSameDirectionWithLanelets(
+    const lanelet::ConstLanelets & lanelets,
+    const autoware_auto_perception_msgs::msg::DetectedObject & object);
   geometry_msgs::msg::Polygon setFootprint(
     const autoware_auto_perception_msgs::msg::DetectedObject &);
 

perception/detected_object_validation/src/object_lanelet_filter.cpp

@@ -43,6 +43,15 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod
   filter_target_.MOTORCYCLE = declare_parameter<bool>("filter_target_label.MOTORCYCLE", false);
   filter_target_.BICYCLE = declare_parameter<bool>("filter_target_label.BICYCLE", false);
   filter_target_.PEDESTRIAN = declare_parameter<bool>("filter_target_label.PEDESTRIAN", false);
+  // Set filter settings
+  filter_settings_.polygon_overlap_filter =
+    declare_parameter<bool>("filter_settings.polygon_overlap_filter.enabled");
+  filter_settings_.lanelet_direction_filter =
+    declare_parameter<bool>("filter_settings.lanelet_direction_filter.enabled");
+  filter_settings_.lanelet_direction_filter_velocity_yaw_threshold =
+    declare_parameter<double>("filter_settings.lanelet_direction_filter.velocity_yaw_threshold");
+  filter_settings_.lanelet_direction_filter_object_speed_threshold =
+    declare_parameter<double>("filter_settings.lanelet_direction_filter.object_speed_threshold");
 
   // Set publisher/subscriber
   map_sub_ = this->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
@@ -97,27 +106,13 @@ void ObjectLaneletFilterNode::objectCallback(
   lanelet::ConstLanelets intersected_shoulder_lanelets =
     getIntersectedLanelets(convex_hull, shoulder_lanelets_);
 
-  int index = 0;
-  for (const auto & object : transformed_objects.objects) {
-    const auto footprint = setFootprint(object);
-    const auto & label = object.classification.front().label;
-    if (filter_target_.isTarget(label)) {
-      Polygon2d polygon;
-      for (const auto & point : footprint.points) {
-        const geometry_msgs::msg::Point32 point_transformed =
-          tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose);
-        polygon.outer().emplace_back(point_transformed.x, point_transformed.y);
-      }
-      polygon.outer().push_back(polygon.outer().front());
-      if (isPolygonOverlapLanelets(polygon, intersected_road_lanelets)) {
-        output_object_msg.objects.emplace_back(input_msg->objects.at(index));
-      } else if (isPolygonOverlapLanelets(polygon, intersected_shoulder_lanelets)) {
-        output_object_msg.objects.emplace_back(input_msg->objects.at(index));
-      }
-    } else {
-      output_object_msg.objects.emplace_back(input_msg->objects.at(index));
-    }
-    ++index;
+  // filtering process
+  for (size_t index = 0; index < transformed_objects.objects.size(); ++index) {
+    const auto & transformed_object = transformed_objects.objects.at(index);
+    const auto & input_object = input_msg->objects.at(index);
+    filterObject(
+      transformed_object, input_object, intersected_road_lanelets, intersected_shoulder_lanelets,
+      output_object_msg);
   }
   object_pub_->publish(output_object_msg);
   published_time_publisher_->publish_if_subscribed(object_pub_, output_object_msg.header.stamp);
@@ -132,6 +127,55 @@ void ObjectLaneletFilterNode::objectCallback(
     "debug/pipeline_latency_ms", pipeline_latency);
 }
 
+bool ObjectLaneletFilterNode::filterObject(
+  const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object,
+  const autoware_auto_perception_msgs::msg::DetectedObject & input_object,
+  const lanelet::ConstLanelets & intersected_road_lanelets,
+  const lanelet::ConstLanelets & intersected_shoulder_lanelets,
+  autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg)
+{
+  const auto & label = transformed_object.classification.front().label;
+  if (filter_target_.isTarget(label)) {
+    bool filter_pass = true;
+    // 1. is polygon overlap with road lanelets or shoulder lanelets
+    if (filter_settings_.polygon_overlap_filter) {
+      Polygon2d polygon;
+      const auto footprint = setFootprint(transformed_object);
+      for (const auto & point : footprint.points) {
+        const geometry_msgs::msg::Point32 point_transformed = tier4_autoware_utils::transformPoint(
+          point, transformed_object.kinematics.pose_with_covariance.pose);
+        polygon.outer().emplace_back(point_transformed.x, point_transformed.y);
+      }
+      polygon.outer().push_back(polygon.outer().front());
+      const bool is_polygon_overlap =
+        isPolygonOverlapLanelets(polygon, intersected_road_lanelets) ||
+        isPolygonOverlapLanelets(polygon, intersected_shoulder_lanelets);
+      filter_pass = filter_pass && is_polygon_overlap;
+    }
+
+    // 2. check if objects velocity is the same with the lanelet direction
+    const bool orientation_not_available =
+      transformed_object.kinematics.orientation_availability ==
+      autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE;
+    if (filter_settings_.lanelet_direction_filter && !orientation_not_available) {
+      const bool is_same_direction =
+        isSameDirectionWithLanelets(intersected_road_lanelets, transformed_object) ||
+        isSameDirectionWithLanelets(intersected_shoulder_lanelets, transformed_object);
+      filter_pass = filter_pass && is_same_direction;
+    }
+
+    // push back to output object
+    if (filter_pass) {
+      output_object_msg.objects.emplace_back(input_object);
+      return true;
+    }
+  } else {
+    output_object_msg.objects.emplace_back(input_object);
+    return true;
+  }
+  return false;
+}
+
 geometry_msgs::msg::Polygon ObjectLaneletFilterNode::setFootprint(
   const autoware_auto_perception_msgs::msg::DetectedObject & detected_object)
 {
@@ -201,6 +245,42 @@ bool ObjectLaneletFilterNode::isPolygonOverlapLanelets(
   return false;
 }
 
+bool ObjectLaneletFilterNode::isSameDirectionWithLanelets(
+  const lanelet::ConstLanelets & lanelets,
+  const autoware_auto_perception_msgs::msg::DetectedObject & object)
+{
+  const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
+  const double object_velocity_norm = std::hypot(
+    object.kinematics.twist_with_covariance.twist.linear.x,
+    object.kinematics.twist_with_covariance.twist.linear.y);
+  const double object_velocity_yaw = std::atan2(
+                                       object.kinematics.twist_with_covariance.twist.linear.y,
+                                       object.kinematics.twist_with_covariance.twist.linear.x) +
+                                     object_yaw;
+
+  if (object_velocity_norm < filter_settings_.lanelet_direction_filter_object_speed_threshold) {
+    return true;
+  }
+  for (const auto & lanelet : lanelets) {
+    const bool is_in_lanelet =
+      lanelet::utils::isInLanelet(object.kinematics.pose_with_covariance.pose, lanelet, 0.0);
+    if (!is_in_lanelet) {
+      continue;
+    }
+    const double lane_yaw = lanelet::utils::getLaneletAngle(
+      lanelet, object.kinematics.pose_with_covariance.pose.position);
+    const double delta_yaw = object_velocity_yaw - lane_yaw;
+    const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw);
+    const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw);
+
+    if (abs_norm_delta_yaw < filter_settings_.lanelet_direction_filter_velocity_yaw_threshold) {
+      return true;
+    }
+  }
+
+  return false;
+}
+
 }  // namespace object_lanelet_filter
 
 #include <rclcpp_components/register_node_macro.hpp>