From f3e76a8affaaa67e4edcdadff1a4ea74dad1b67a Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 26 Nov 2024 09:11:52 +0900 Subject: [PATCH] fix(surround_obstacle_checker)!: remove stop reason (#9450) fix(surround_obstacle_checker): remove stop reason Signed-off-by: satoshi-ota --- .../README.md | 3 -- .../surround_obstacle_checker.launch.xml | 1 - .../src/debug_marker.cpp | 30 ----------------- .../src/debug_marker.hpp | 6 ---- .../src/node.cpp | 33 ------------------- .../src/node.hpp | 1 - .../surround_obstacle_checker-design.ja.md | 3 -- 7 files changed, 77 deletions(-) diff --git a/planning/autoware_surround_obstacle_checker/README.md b/planning/autoware_surround_obstacle_checker/README.md index 1f4bf77145624..fbe749f2eec08 100644 --- a/planning/autoware_surround_obstacle_checker/README.md +++ b/planning/autoware_surround_obstacle_checker/README.md @@ -34,8 +34,6 @@ else endif -:Publish stop reason; - stop @enduml ``` @@ -93,7 +91,6 @@ As mentioned in stop condition section, it prevents chattering by changing thres | `~/output/velocity_limit_clear_command` | `tier4_planning_msgs::msg::VelocityLimitClearCommand` | Velocity limit clear command | | `~/output/max_velocity` | `tier4_planning_msgs::msg::VelocityLimit` | Velocity limit command | | `~/output/no_start_reason` | `diagnostic_msgs::msg::DiagnosticStatus` | No start reason | -| `~/output/stop_reasons` | `tier4_planning_msgs::msg::StopReasonArray` | Stop reasons | | `~/debug/marker` | `visualization_msgs::msg::MarkerArray` | Marker for visualization | | `~/debug/footprint` | `geometry_msgs::msg::PolygonStamped` | Ego vehicle base footprint for visualization | | `~/debug/footprint_offset` | `geometry_msgs::msg::PolygonStamped` | Ego vehicle footprint with `surround_check_distance` offset for visualization | diff --git a/planning/autoware_surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml b/planning/autoware_surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml index 0ba7d29090916..c7e440f6eb3e1 100644 --- a/planning/autoware_surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml +++ b/planning/autoware_surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml @@ -10,7 +10,6 @@ - diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp index 57d9cc8bc67a7..6a5883fbd660d 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp @@ -75,7 +75,6 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( clock_(clock) { debug_viz_pub_ = node.create_publisher("~/debug/marker", 1); - stop_reason_pub_ = node.create_publisher("~/output/stop_reasons", 1); velocity_factor_pub_ = node.create_publisher("/planning/velocity_factors/surround_obstacle", 1); vehicle_footprint_pub_ = node.create_publisher("~/debug/footprint", 1); @@ -142,8 +141,6 @@ void SurroundObstacleCheckerDebugNode::publish() debug_viz_pub_->publish(visualization_msg); /* publish stop reason for autoware api */ - const auto stop_reason_msg = makeStopReasonArray(); - stop_reason_pub_->publish(stop_reason_msg); const auto velocity_factor_msg = makeVelocityFactorArray(); velocity_factor_pub_->publish(velocity_factor_msg); @@ -171,33 +168,6 @@ MarkerArray SurroundObstacleCheckerDebugNode::makeVisualizationMarker() return msg; } -StopReasonArray SurroundObstacleCheckerDebugNode::makeStopReasonArray() -{ - // create header - std_msgs::msg::Header header; - header.frame_id = "map"; - header.stamp = this->clock_->now(); - - // create stop reason stamped - StopReason stop_reason_msg; - stop_reason_msg.reason = StopReason::SURROUND_OBSTACLE_CHECK; - StopFactor stop_factor; - - if (stop_pose_ptr_ != nullptr) { - stop_factor.stop_pose = *stop_pose_ptr_; - if (stop_obstacle_point_ptr_ != nullptr) { - stop_factor.stop_factor_points.emplace_back(*stop_obstacle_point_ptr_); - } - stop_reason_msg.stop_factors.emplace_back(stop_factor); - } - - // create stop reason array - StopReasonArray stop_reason_array; - stop_reason_array.header = header; - stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); - return stop_reason_array; -} - VelocityFactorArray SurroundObstacleCheckerDebugNode::makeVelocityFactorArray() { VelocityFactorArray velocity_factor_array; diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp index 17ec2631b2dad..b2c350c1b4698 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -39,9 +38,6 @@ using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::VelocityFactor; using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using geometry_msgs::msg::PolygonStamped; -using tier4_planning_msgs::msg::StopFactor; -using tier4_planning_msgs::msg::StopReason; -using tier4_planning_msgs::msg::StopReasonArray; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -69,7 +65,6 @@ class SurroundObstacleCheckerDebugNode private: rclcpp::Publisher::SharedPtr debug_viz_pub_; - rclcpp::Publisher::SharedPtr stop_reason_pub_; rclcpp::Publisher::SharedPtr velocity_factor_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_pub_; @@ -85,7 +80,6 @@ class SurroundObstacleCheckerDebugNode geometry_msgs::msg::Pose self_pose_; MarkerArray makeVisualizationMarker(); - StopReasonArray makeStopReasonArray(); VelocityFactorArray makeVelocityFactorArray(); PolygonStamped boostPolygonToPolygonStamped(const Polygon2d & boost_polygon, const double & z); diff --git a/planning/autoware_surround_obstacle_checker/src/node.cpp b/planning/autoware_surround_obstacle_checker/src/node.cpp index 70e81fa3b1d12..1a938a9275df5 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.cpp +++ b/planning/autoware_surround_obstacle_checker/src/node.cpp @@ -57,34 +57,6 @@ using autoware::universe_utils::createPoint; using autoware::universe_utils::pose2transform; using autoware_perception_msgs::msg::ObjectClassification; -namespace -{ -std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) -{ - const std::string json_dumps_pose = - (boost::format( - R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % - pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % - pose.orientation.y % pose.orientation.z) - .str(); - return json_dumps_pose; -} - -diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( - const std::string & no_start_reason, const geometry_msgs::msg::Pose & stop_pose) -{ - diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag; - diagnostic_msgs::msg::KeyValue no_start_reason_diag_kv; - no_start_reason_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - no_start_reason_diag.name = "no_start_reason"; - no_start_reason_diag.message = no_start_reason; - no_start_reason_diag_kv.key = "no_start_pose"; - no_start_reason_diag_kv.value = jsonDumpsPose(stop_pose); - no_start_reason_diag.values.push_back(no_start_reason_diag_kv); - return no_start_reason_diag; -} -} // namespace - SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptions & node_options) : Node("surround_obstacle_checker_node", node_options) { @@ -104,8 +76,6 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); // Publishers - pub_stop_reason_ = - this->create_publisher("~/output/no_start_reason", 1); pub_clear_velocity_limit_ = this->create_publisher( "~/output/velocity_limit_clear_command", rclcpp::QoS{1}.transient_local()); pub_velocity_limit_ = this->create_publisher( @@ -256,10 +226,8 @@ void SurroundObstacleCheckerNode::onTimer() debug_ptr_->pushObstaclePoint(nearest_obstacle.value().second, PointType::NoStart); } - diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag; if (state_ == State::STOP) { debug_ptr_->pushPose(odometry_ptr_->pose.pose, PoseType::NoStart); - no_start_reason_diag = makeStopReasonDiag("obstacle", odometry_ptr_->pose.pose); } tier4_debug_msgs::msg::Float64Stamped processing_time_msg; @@ -267,7 +235,6 @@ void SurroundObstacleCheckerNode::onTimer() processing_time_msg.data = stop_watch.toc(); pub_processing_time_->publish(processing_time_msg); - pub_stop_reason_->publish(no_start_reason_diag); debug_ptr_->publish(); } diff --git a/planning/autoware_surround_obstacle_checker/src/node.hpp b/planning/autoware_surround_obstacle_checker/src/node.hpp index 480a937a4a909..17eb2979e6809 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.hpp +++ b/planning/autoware_surround_obstacle_checker/src/node.hpp @@ -104,7 +104,6 @@ class SurroundObstacleCheckerNode : public rclcpp::Node sub_pointcloud_{this, "~/input/pointcloud", autoware::universe_utils::SingleDepthSensorQoS()}; autoware::universe_utils::InterProcessPollingSubscriber sub_dynamic_objects_{ this, "~/input/objects"}; - rclcpp::Publisher::SharedPtr pub_stop_reason_; rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; rclcpp::Publisher::SharedPtr pub_velocity_limit_; rclcpp::Publisher::SharedPtr pub_processing_time_; diff --git a/planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja.md b/planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja.md index f8a5f29e064f7..b19f556e59830 100644 --- a/planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja.md +++ b/planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja.md @@ -34,8 +34,6 @@ else endif -:Publish stop reason; - stop @enduml ``` @@ -93,7 +91,6 @@ Stop condition の項で述べたように、状態によって障害物判定 | `~/output/velocity_limit_clear_command` | `tier4_planning_msgs::msg::VelocityLimitClearCommand` | Velocity limit clear command | | `~/output/max_velocity` | `tier4_planning_msgs::msg::VelocityLimit` | Velocity limit command | | `~/output/no_start_reason` | `diagnostic_msgs::msg::DiagnosticStatus` | No start reason | -| `~/output/stop_reasons` | `tier4_planning_msgs::msg::StopReasonArray` | Stop reasons | | `~/debug/marker` | `visualization_msgs::msg::MarkerArray` | Marker for visualization | ## Parameters