Make filtering objects behind ego configurable

trajectory_optimization/README.md

@@ -66,6 +66,7 @@ flowchart LR
 | `standstill_threshold` | `float` | `0.45` | Threshold for standstill detection [m/s]. If the velocities of all states are below this threshold, publish standstill trajectory |
 | `high_level_stabilization` | `bool` | `false` | Use high-level stabilization strategy for init state (= init with current EgoData) |
 | `consider_objects` | `int` | `2` | consider objects in optimization: 0 = none, 1 = static (no prediction), 2 = dynamic (with prediction) |
+| `ignore_objects_behind_ego` | `bool` | `true` | Exclude objects with reference-point x <= 0 in vehicle_frame_id |
 | `min_prediction_probability` | `float` | `0.0` | Minimum probability for predicted object states to be considered |
 | `consider_boundaries` | `int` | `1` | consider route boundaries in optimization: 0 = no, 1 = suggested lane, 2 = including adjacent, 3 = drivable space |
 | `bi_level_dV` | `float` | `2.0` | Threshold for bi-level stabilization: maximum velocity difference [m/s] |

trajectory_optimization/config/example_params_ackermann.yml

@@ -9,6 +9,7 @@
     standstill_threshold: 0.3       # threshold for standstill detection [m/s]. If all state velocities are below this threshold, publish standstill trajectory
     high_level_stabilization: False # init first trajectory point with: current EgoData (True); interpolation of last trajectory -> bi-level (False)
     consider_objects: 2             # consider objects in optimization: 0 = no, 1 = static (no prediction), 2 = dynamic (with prediction)
+    ignore_objects_behind_ego: True # exclude objects with reference-point x <= 0 in vehicle_frame_id
     min_prediction_probability: 0.0 # consider only predictions with probability > this threshold
     consider_boundaries: 3          # consider route boundaries in optimization: 0 = no, 1 = suggested lane, 2 = including adjacent, 3 = drivable space
     verbose: False                  # print debug infromation and solver statistics

trajectory_optimization/config/example_params_rws.yml

@@ -9,6 +9,7 @@
     standstill_threshold: 0.3       # threshold for standstill detection [m/s]. If all state velocities are below this threshold, publish standstill trajectory
     high_level_stabilization: False # init first trajectory point with: current EgoData (True); interpolation of last trajectory -> bi-level (False)
     consider_objects: 2             # consider objects in optimization: 0 = no, 1 = static (no prediction), 2 = dynamic (with prediction)
+    ignore_objects_behind_ego: True # exclude objects with reference-point x <= 0 in vehicle_frame_id
     min_prediction_probability: 0.0 # consider only predictions with probability > this threshold
     consider_boundaries: 3          # consider route boundaries in optimization: 0 = no, 1 = suggested lane, 2 = including adjacent, 3 = drivable space
     verbose: False                  # print debug infromation and solver statistics

trajectory_optimization/include/trajectory_optimization/trajectory_optimization_node.hpp

@@ -258,12 +258,15 @@ class TrajectoryOptimizationNode : public rclcpp::Node {
   void setOcpParameters(const perception_msgs::msg::EgoData& ego_data, const perception_msgs::msg::ObjectList& object_list);
 
   /**
-   * @brief Keeps the nearest forward objects and discards the remaining entries.
+   * @brief Keeps the nearest objects, optionally excluding those behind the ego vehicle.
    *
    * @param[in,out] object_list Object list to filter.
    * @param[in] n_objects Maximum number of objects to retain.
+   * @param[in] ignore_objects_behind_ego Exclude objects with reference-point x <= 0 in the optimizer frame.
    */
-  static void keepNClosestObjects(perception_msgs::msg::ObjectList& object_list, const int n_objects);
+  static void keepNClosestObjects(perception_msgs::msg::ObjectList& object_list,
+                                  const int n_objects,
+                                  const bool ignore_objects_behind_ego);
 
   /**
    * @brief Enables the configured object and boundary constraints or temporarily disables all of them.
@@ -393,6 +396,7 @@ class TrajectoryOptimizationNode : public rclcpp::Node {
   double standstill_threshold_ = 0.45;
   bool high_level_stabilization_ = false;
   uint8_t consider_objects_ = CONSIDER_OBJECTS::PREDICTED_OBJECTS;
+  bool ignore_objects_behind_ego_ = true;
   uint8_t consider_boundaries_ = CONSIDER_BOUNDARIES::SUGGESTED_LANE;
   bool run_as_callback_ = false;
 

trajectory_optimization/src/trajectory_optimization_node.cpp

@@ -78,6 +78,8 @@ TrajectoryOptimizationNode::TrajectoryOptimizationNode(const std::string node_na
   this->declareAndLoadParameter(
       "consider_objects", consider_objects_,
       "consider objects in optimization: 0 = none, 1 = static (no prediction), 2 = dynamic (with prediction)");
+  this->declareAndLoadParameter("ignore_objects_behind_ego", ignore_objects_behind_ego_,
+                                "Exclude objects with reference-point x <= 0 in vehicle_frame_id");
   this->declareAndLoadParameter("min_prediction_probability", min_prediction_probability_,
                                 "Minimum probability for predicted object states to be considered", true, false, false, 0.0, 1.0);
   this->declareAndLoadParameter(
@@ -600,7 +602,7 @@ bool TrajectoryOptimizationNode::updateOcpInputs(const perception_msgs::msg::Ego
     } else {
       tf_object_list = object_list;
     }
-    keepNClosestObjects(tf_object_list, static_cast<int>(p_objects_shape_[0]));
+    keepNClosestObjects(tf_object_list, static_cast<int>(p_objects_shape_[0]), ignore_objects_behind_ego_);
     // route
     if (!route.route_elements.empty() && route.header.frame_id != vehicle_frame_id_) {
       tf_route = tf2_buffer_->transform(route, vehicle_frame_id_, tf2_ros::fromMsg(ego_data.header.stamp),

trajectory_optimization/src/utils.cpp

@@ -85,7 +85,9 @@ bool TrajectoryOptimizationNode::trajectory2outputFrame(trajectory_planning_msgs
   return true;
 }
 
-void TrajectoryOptimizationNode::keepNClosestObjects(perception_msgs::msg::ObjectList& object_list, const int n_objects) {
+void TrajectoryOptimizationNode::keepNClosestObjects(perception_msgs::msg::ObjectList& object_list,
+                                                     const int n_objects,
+                                                     const bool ignore_objects_behind_ego) {
   // sort object indices by distance to ego vehicle
   std::vector<size_t> indices_sorted_by_distance(object_list.objects.size());
   std::iota(indices_sorted_by_distance.begin(), indices_sorted_by_distance.end(), 0);
@@ -96,11 +98,11 @@ void TrajectoryOptimizationNode::keepNClosestObjects(perception_msgs::msg::Objec
            std::hypot(perception_msgs::object_access::getX(second_object), perception_msgs::object_access::getY(second_object));
   });
 
-  // keep only the closest n_objects that are in front of the ego vehicle
+  // keep only the closest n_objects, optionally excluding objects behind the ego vehicle
   std::vector<perception_msgs::msg::Object> closest_objects;
   closest_objects.reserve(std::min(static_cast<size_t>(n_objects), object_list.objects.size()));
   for (const size_t index : indices_sorted_by_distance) {
-    if (perception_msgs::object_access::getX(object_list.objects[index]) <= 0.0) continue;
+    if (ignore_objects_behind_ego && perception_msgs::object_access::getX(object_list.objects[index]) <= 0.0) continue;
     closest_objects.push_back(object_list.objects[index]);
     if (closest_objects.size() >= static_cast<size_t>(n_objects)) break;
   }