diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 1f25372c14d71..83727574f39fb 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -45,7 +45,7 @@ common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomo common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_utils/** kotaro.uetake@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -control/autoware_autonomous_emergency_braking/** daniel.sanchez@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +control/autoware_autonomous_emergency_braking/** daniel.sanchez@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp kyoichi.sugahara@tier4.jp control/autoware_control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/autoware_external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/autoware_joy_controller/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index f040235f3f052..db27ca1b4f699 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -192,6 +192,7 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t | Name | Unit | Type | Description | Default value | | :-------------------------------- | :----- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| publish_debug_markers | [-] | bool | flag to publish debug markers | true | | publish_debug_pointcloud | [-] | bool | flag to publish the point cloud used for debugging | false | | use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | | use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 482ddc50766f8..25db9448f758b 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -14,6 +14,7 @@ # Debug publish_debug_pointcloud: false + publish_debug_markers: true # Point cloud partitioning detection_range_min_height: 0.0 diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index f6df46440d841..9fd6d0192439c 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -329,6 +329,7 @@ class AEB : public rclcpp::Node // member variables bool publish_debug_pointcloud_; + bool publish_debug_markers_; bool use_predicted_trajectory_; bool use_imu_path_; bool use_pointcloud_data_; diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp index 4a6bc0ce2c11f..d2ec1763ca4dd 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/utils.hpp @@ -33,8 +33,6 @@ #include #endif -#include - namespace autoware::motion::control::autonomous_emergency_braking::utils { using autoware::universe_utils::Polygon2d; diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index c822adb510805..0c64b9498486a 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -8,6 +8,7 @@ Tomoya Kimura Mamoru Sobue Daniel Sanchez + Kyoichi Sugahara Apache License 2.0 diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index fb0fc2d043cc5..69bedd02d201b 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -41,7 +42,9 @@ #include #include +#include #include +#include #ifdef ROS_DISTRO_GALACTIC #include #include @@ -131,6 +134,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) } // parameter publish_debug_pointcloud_ = declare_parameter("publish_debug_pointcloud"); + publish_debug_markers_ = declare_parameter("publish_debug_markers"); use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); use_imu_path_ = declare_parameter("use_imu_path"); use_pointcloud_data_ = declare_parameter("use_pointcloud_data"); @@ -182,6 +186,7 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( { using autoware::universe_utils::updateParam; updateParam(parameters, "publish_debug_pointcloud", publish_debug_pointcloud_); + updateParam(parameters, "publish_debug_markers", publish_debug_markers_); updateParam(parameters, "use_predicted_trajectory", use_predicted_trajectory_); updateParam(parameters, "use_imu_path", use_imu_path_); updateParam(parameters, "use_pointcloud_data", use_pointcloud_data_); @@ -378,7 +383,9 @@ void AEB::onCheckCollision(DiagnosticStatusWrapper & stat) stat.addf("RSS", "%.2f", data.rss); stat.addf("Distance", "%.2f", data.distance_to_object); stat.addf("Object Speed", "%.2f", data.velocity); - addCollisionMarker(data, debug_markers); + if (publish_debug_markers_) { + addCollisionMarker(data, debug_markers); + } } else { const std::string error_msg = "[AEB]: No Collision"; const auto diag_level = DiagnosticStatus::OK; @@ -455,7 +462,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) }); // Add debug markers - { + if (publish_debug_markers_) { const auto [color_r, color_g, color_b, color_a] = debug_colors; addMarker( this->get_clock()->now(), path, ego_polys, objects, closest_object_point, color_r, color_g, @@ -896,6 +903,33 @@ void AEB::addCollisionMarker(const ObjectData & data, MarkerArray & debug_marker 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::TransformStamped tf_current_pose; + geometry_msgs::msg::Pose p; + try { + tf_current_pose = tf_buffer_.lookupTransform( + "map", "base_link", rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(get_logger(), "%s", ex.what()); + return std::nullopt; + } + + p.orientation = tf_current_pose.transform.rotation; + p.position.x = tf_current_pose.transform.translation.x; + p.position.y = tf_current_pose.transform.translation.y; + p.position.z = tf_current_pose.transform.translation.z; + return std::make_optional(p); + }); + + if (ego_map_pose.has_value()) { + const double base_to_front_offset = vehicle_info_.max_longitudinal_offset_m; + const auto ego_front_pose = autoware::universe_utils::calcOffsetPose( + 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); + } } } // namespace autoware::motion::control::autonomous_emergency_braking