feat(autonomous_emergency_braking): add info marker and override for …

…state (autowarefoundation#8312)

add info marker and override for state

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml

@@ -6,6 +6,7 @@
     use_pointcloud_data: true
     use_predicted_object_data: true
     use_object_velocity_calculation: true
+    check_autoware_state: true
     min_generated_path_length: 0.5
     imu_prediction_time_horizon: 1.5
     imu_prediction_time_interval: 0.1

control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp

@@ -260,7 +260,8 @@ class AEB : public rclcpp::Node
     this, "/autoware/state"};
   // publisher
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_obstacle_pointcloud_;
-  rclcpp::Publisher<MarkerArray>::SharedPtr debug_ego_path_publisher_;  // debug
+  rclcpp::Publisher<MarkerArray>::SharedPtr debug_marker_publisher_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr info_marker_publisher_;
 
   // timer
   rclcpp::TimerBase::SharedPtr timer_;
@@ -308,6 +309,8 @@ class AEB : public rclcpp::Node
 
   void addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers);
 
+  void addVirtualStopWallMarker(MarkerArray & markers);
+
   std::optional<double> calcObjectSpeedFromHistory(
     const ObjectData & closest_object, const Path & path, const double current_ego_speed);
 
@@ -335,6 +338,7 @@ class AEB : public rclcpp::Node
   bool use_pointcloud_data_;
   bool use_predicted_object_data_;
   bool use_object_velocity_calculation_;
+  bool check_autoware_state_;
   double path_footprint_extra_margin_;
   double detection_range_min_height_;
   double detection_range_max_height_margin_;

control/autoware_autonomous_emergency_braking/src/node.cpp

@@ -125,7 +125,8 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
   {
     pub_obstacle_pointcloud_ =
       this->create_publisher<sensor_msgs::msg::PointCloud2>("~/debug/obstacle_pointcloud", 1);
-    debug_ego_path_publisher_ = this->create_publisher<MarkerArray>("~/debug/markers", 1);
+    debug_marker_publisher_ = this->create_publisher<MarkerArray>("~/debug/markers", 1);
+    info_marker_publisher_ = this->create_publisher<MarkerArray>("~/info/markers", 1);
   }
   // Diagnostics
   {
@@ -140,6 +141,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options)
   use_pointcloud_data_ = declare_parameter<bool>("use_pointcloud_data");
   use_predicted_object_data_ = declare_parameter<bool>("use_predicted_object_data");
   use_object_velocity_calculation_ = declare_parameter<bool>("use_object_velocity_calculation");
+  check_autoware_state_ = declare_parameter<bool>("check_autoware_state");
   path_footprint_extra_margin_ = declare_parameter<double>("path_footprint_extra_margin");
   detection_range_min_height_ = declare_parameter<double>("detection_range_min_height");
   detection_range_max_height_margin_ =
@@ -193,6 +195,7 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter(
   updateParam<bool>(parameters, "use_predicted_object_data", use_predicted_object_data_);
   updateParam<bool>(
     parameters, "use_object_velocity_calculation", use_object_velocity_calculation_);
+  updateParam<bool>(parameters, "check_autoware_state", check_autoware_state_);
   updateParam<double>(parameters, "path_footprint_extra_margin", path_footprint_extra_margin_);
   updateParam<double>(parameters, "detection_range_min_height", detection_range_min_height_);
   updateParam<double>(
@@ -363,7 +366,7 @@ bool AEB::fetchLatestData()
   }
 
   autoware_state_ = sub_autoware_state_.takeData();
-  if (!autoware_state_) {
+  if (check_autoware_state_ && !autoware_state_) {
     return missing("autoware_state");
   }
 
@@ -373,6 +376,7 @@ bool AEB::fetchLatestData()
 void AEB::onCheckCollision(DiagnosticStatusWrapper & stat)
 {
   MarkerArray debug_markers;
+  MarkerArray info_markers;
   checkCollision(debug_markers);
 
   if (!collision_data_keeper_.checkCollisionExpired()) {
@@ -386,14 +390,16 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat)
     if (publish_debug_markers_) {
       addCollisionMarker(data, debug_markers);
     }
+    addVirtualStopWallMarker(info_markers);
   } else {
     const std::string error_msg = "[AEB]: No Collision";
     const auto diag_level = DiagnosticStatus::OK;
     stat.summary(diag_level, error_msg);
   }
 
   // publish debug markers
-  debug_ego_path_publisher_->publish(debug_markers);
+  debug_marker_publisher_->publish(debug_markers);
+  info_marker_publisher_->publish(info_markers);
 }
 
 bool AEB::checkCollision(MarkerArray & debug_markers)
@@ -406,7 +412,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers)
   }
 
   // if not driving, disable aeb
-  if (autoware_state_->state != AutowareState::DRIVING) {
+  if (check_autoware_state_ && autoware_state_->state != AutowareState::DRIVING) {
     return false;
   }
 
@@ -890,20 +896,17 @@ void AEB::addMarker(
       "Object velocity: " + std::to_string(obj.velocity) + " [m/s]\n";
     closest_object_velocity_marker_array.text +=
       "Object relative velocity to ego: " + std::to_string(obj.velocity - std::abs(ego_velocity)) +
-      " [m/s]";
+      " [m/s]\n";
+    closest_object_velocity_marker_array.text +=
+      "Object distance to ego: " + std::to_string(obj.distance_to_object) + " [m]\n";
+    closest_object_velocity_marker_array.text +=
+      "RSS distance: " + std::to_string(obj.rss) + " [m]";
     debug_markers.markers.push_back(closest_object_velocity_marker_array);
   }
 }
 
-void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers)
+void AEB::addVirtualStopWallMarker(MarkerArray & markers)
 {
-  auto point_marker = autoware::universe_utils::createDefaultMarker(
-    "base_link", data.stamp, "collision_point", 0, Marker::SPHERE,
-    autoware::universe_utils::createMarkerScale(0.3, 0.3, 0.3),
-    autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3));
-  point_marker.pose.position = data.position;
-  debug_markers.markers.push_back(point_marker);
-
   const auto ego_map_pose = std::invoke([this]() -> std::optional<geometry_msgs::msg::Pose> {
     geometry_msgs::msg::TransformStamped tf_current_pose;
     geometry_msgs::msg::Pose p;
@@ -928,10 +931,20 @@ void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_marker
       ego_map_pose.value(), base_to_front_offset, 0.0, 0.0, 0.0);
     const auto virtual_stop_wall = autoware::motion_utils::createStopVirtualWallMarker(
       ego_front_pose, "autonomous_emergency_braking", this->now(), 0);
-    autoware::universe_utils::appendMarkerArray(virtual_stop_wall, &debug_markers);
+    autoware::universe_utils::appendMarkerArray(virtual_stop_wall, &markers);
   }
 }
 
+void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_markers)
+{
+  auto point_marker = autoware::universe_utils::createDefaultMarker(
+    "base_link", data.stamp, "collision_point", 0, Marker::SPHERE,
+    autoware::universe_utils::createMarkerScale(0.3, 0.3, 0.3),
+    autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.3));
+  point_marker.pose.position = data.position;
+  debug_markers.markers.push_back(point_marker);
+}
+
 }  // namespace autoware::motion::control::autonomous_emergency_braking
 
 #include <rclcpp_components/register_node_macro.hpp>