diff --git a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp index 14e8271a19b5b..77fafdf7ddeea 100644 --- a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp @@ -16,12 +16,15 @@ #define SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_ #include "behavior_velocity_planner/planner_data.hpp" +#include "velocity_factor_interface.hpp" #include #include #include #include +#include +#include #include #include #include @@ -100,6 +103,9 @@ class SceneModuleInterface bool isSafe() const { return safe_; } double getDistance() const { return distance_; } + void resetVelocityFactor() { velocity_factor_.reset(); } + VelocityFactor getVelocityFactor() const { return velocity_factor_.get(); } + protected: const int64_t module_id_; bool activated_; @@ -110,6 +116,7 @@ class SceneModuleInterface std::shared_ptr planner_data_; boost::optional infrastructure_command_; boost::optional first_stop_path_point_index_; + VelocityFactorInterface velocity_factor_; void setSafe(const bool safe) { safe_ = safe; } void setDistance(const double distance) { distance_ = distance; } @@ -150,6 +157,8 @@ class SceneModuleManagerInterface } pub_virtual_wall_ = node.create_publisher( std::string("~/virtual_wall/") + module_name, 5); + pub_velocity_factor_ = node.create_publisher( + std::string("/planning/velocity_factors/") + module_name, 1); pub_stop_reason_ = node.create_publisher("~/output/stop_reasons", 1); pub_infrastructure_commands_ = @@ -188,8 +197,11 @@ class SceneModuleManagerInterface visualization_msgs::msg::MarkerArray debug_marker_array; visualization_msgs::msg::MarkerArray virtual_wall_marker_array; tier4_planning_msgs::msg::StopReasonArray stop_reason_array; + autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; stop_reason_array.header.frame_id = "map"; stop_reason_array.header.stamp = clock_->now(); + velocity_factor_array.header.frame_id = "map"; + velocity_factor_array.header.stamp = clock_->now(); tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; infrastructure_command_array.stamp = clock_->now(); @@ -197,9 +209,15 @@ class SceneModuleManagerInterface first_stop_path_point_index_ = static_cast(path->points.size()) - 1; for (const auto & scene_module : scene_modules_) { tier4_planning_msgs::msg::StopReason stop_reason; + scene_module->resetVelocityFactor(); scene_module->setPlannerData(planner_data_); scene_module->modifyPathVelocity(path, &stop_reason); + // The velocity factor must be called after modifyPathVelocity. + const auto velocity_factor = scene_module->getVelocityFactor(); + if (velocity_factor.type != VelocityFactor::UNKNOWN) { + velocity_factor_array.factors.emplace_back(velocity_factor); + } if (stop_reason.reason != "") { stop_reason_array.stop_reasons.emplace_back(stop_reason); } @@ -224,6 +242,7 @@ class SceneModuleManagerInterface if (!stop_reason_array.stop_reasons.empty()) { pub_stop_reason_->publish(stop_reason_array); } + pub_velocity_factor_->publish(velocity_factor_array); pub_infrastructure_commands_->publish(infrastructure_command_array); pub_debug_->publish(debug_marker_array); if (is_publish_debug_path_) { @@ -311,6 +330,8 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_debug_; rclcpp::Publisher::SharedPtr pub_debug_path_; rclcpp::Publisher::SharedPtr pub_stop_reason_; + rclcpp::Publisher::SharedPtr + pub_velocity_factor_; rclcpp::Publisher::SharedPtr pub_infrastructure_commands_; diff --git a/planning/behavior_velocity_planner/include/scene_module/velocity_factor_interface.hpp b/planning/behavior_velocity_planner/include/scene_module/velocity_factor_interface.hpp new file mode 100644 index 0000000000000..cff6e54b4f134 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/velocity_factor_interface.hpp @@ -0,0 +1,68 @@ + +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SCENE_MODULE__VELOCITY_FACTOR_INTERFACE_HPP_ +#define SCENE_MODULE__VELOCITY_FACTOR_INTERFACE_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ + +using autoware_adapi_v1_msgs::msg::VelocityFactor; +using autoware_adapi_v1_msgs::msg::VelocityFactorArray; +using geometry_msgs::msg::Pose; +using VelocityFactorType = VelocityFactor::_type_type; +using VelocityFactorStatus = VelocityFactor::_status_type; + +class VelocityFactorInterface +{ +public: + VelocityFactorInterface() { type_ = VelocityFactor::UNKNOWN; } + + VelocityFactor get() const { return velocity_factor_; } + void init(const VelocityFactorType type) { type_ = type; } + void reset() { velocity_factor_.type = VelocityFactor::UNKNOWN; } + + template + void set( + const T & 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; + velocity_factor_.type = type_; + velocity_factor_.pose = stop_pose; + velocity_factor_.distance = motion_utils::calcSignedArcLength(points, curr_point, stop_point); + velocity_factor_.status = status; + velocity_factor_.detail = detail; + } + +private: + VelocityFactorType type_; + VelocityFactor velocity_factor_; +}; + +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__VELOCITY_FACTOR_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp index 1d49c74afbd83..130e4a1715fde 100644 --- a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp @@ -99,6 +99,10 @@ class VirtualTrafficLightModule : public SceneModuleInterface void setStopReason(const Pose & stop_pose, StopReason * stop_reason); + void setVelocityFactor( + const geometry_msgs::msg::Pose & stop_pose, + autoware_adapi_v1_msgs::msg::VelocityFactor * velocity_factor); + boost::optional getPathIndexOfFirstEndLine(); bool isBeforeStartLine(const size_t end_line_idx); diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index c648e25a055d2..3995866f760c6 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -47,6 +47,7 @@ autoware_cmake + autoware_adapi_v1_msgs autoware_auto_mapping_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp index b831ec6ea2e84..1b64566611cfe 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp @@ -63,6 +63,7 @@ BlindSpotModule::BlindSpotModule( turn_direction_(TurnDirection::INVALID), is_over_pass_judge_line_(false) { + velocity_factor_.init(VelocityFactor::REAR_CHECK); planner_param_ = planner_param; const auto & assigned_lanelet = @@ -183,6 +184,8 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto stop_factor.stop_pose = debug_data_.stop_point_pose; stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets); planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::UNKNOWN); } else { *path = input_path; // reset path } diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp index eac948942f76c..9b40751449dd6 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp @@ -137,6 +137,7 @@ CrosswalkModule::CrosswalkModule( crosswalk_(std::move(crosswalk)), planner_param_(planner_param) { + velocity_factor_.init(VelocityFactor::CROSSWALK); passed_safety_slow_point_ = false; } @@ -218,9 +219,15 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto if (nearest_stop_point) { insertDecelPointWithDebugInfo(nearest_stop_point.get(), 0.0, *path); planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_factor.stop_pose, + VelocityFactor::UNKNOWN); } else if (rtc_stop_point) { insertDecelPointWithDebugInfo(rtc_stop_point.get(), 0.0, *path); planning_utils::appendStopReason(stop_factor_rtc, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_factor_rtc.stop_pose, + VelocityFactor::UNKNOWN); } RCLCPP_INFO_EXPRESSION( diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp index 30efdc27c4197..2cf2c1fb02e47 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp @@ -39,6 +39,7 @@ WalkwayModule::WalkwayModule( state_(State::APPROACH), planner_param_(planner_param) { + velocity_factor_.init(VelocityFactor::SIDEWALK); } boost::optional> WalkwayModule::getStopLine( @@ -121,6 +122,8 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_ stop_factor.stop_pose = stop_pose.get(); stop_factor.stop_factor_points.push_back(path_intersects_.front()); planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_pose.get(), VelocityFactor::UNKNOWN); // use arc length to identify if ego vehicle is in front of walkway stop or not. const double signed_arc_dist_to_stop_point = diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp index d89bb16b8b3ce..8f9bc0a6e92a3 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp @@ -46,6 +46,7 @@ DetectionAreaModule::DetectionAreaModule( state_(State::GO), planner_param_(planner_param) { + velocity_factor_.init(VelocityFactor::USER_DEFINED_DETECTION_AREA); } LineString2d DetectionAreaModule::getStopLineGeometry2d() const @@ -182,6 +183,8 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_factor.stop_pose = stop_point->second; stop_factor.stop_factor_points = obstacle_points; planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_point->second, VelocityFactor::UNKNOWN); } // Create legacy StopReason diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 25bc702d9d6df..d3c20427f118e 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -57,6 +57,7 @@ IntersectionModule::IntersectionModule( const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id), is_go_out_(false) { + velocity_factor_.init(VelocityFactor::INTERSECTION); planner_param_ = planner_param; const auto & assigned_lanelet = @@ -256,14 +257,21 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * debug_data_.stop_point_pose = path->points.at(stop_line_idx_final).point.pose; debug_data_.judge_point_pose = path->points.at(pass_judge_line_idx_final).point.pose; - /* get stop point and stop factor */ - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = debug_data_.stop_point_pose; - const auto stop_factor_conflict = planning_utils::toRosPoints(debug_data_.conflicting_targets); - const auto stop_factor_stuck = planning_utils::toRosPoints(debug_data_.stuck_targets); - stop_factor.stop_factor_points = - planning_utils::concatVector(stop_factor_conflict, stop_factor_stuck); - planning_utils::appendStopReason(stop_factor, stop_reason); + // Get stop point and stop factor + { + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = debug_data_.stop_point_pose; + const auto stop_factor_conflict = + planning_utils::toRosPoints(debug_data_.conflicting_targets); + const auto stop_factor_stuck = planning_utils::toRosPoints(debug_data_.stuck_targets); + stop_factor.stop_factor_points = + planning_utils::concatVector(stop_factor_conflict, stop_factor_stuck); + planning_utils::appendStopReason(stop_factor, stop_reason); + + const auto & stop_pose = path->points.at(stop_line_idx_final).point.pose; + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::UNKNOWN); + } RCLCPP_DEBUG(logger_, "not activated. stop at the line."); RCLCPP_DEBUG(logger_, "===== plan end ====="); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index c76f6c4844a99..d880a6c76dc1f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -39,6 +39,7 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id) { + velocity_factor_.init(VelocityFactor::MERGE); planner_param_ = planner_param; state_machine_.setState(StateMachine::State::STOP); } @@ -111,6 +112,9 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR stop_factor.stop_pose = debug_data_.stop_point_pose; stop_factor.stop_factor_points.emplace_back(debug_data_.first_collision_point); planning_utils::appendStopReason(stop_factor, stop_reason); + const auto & stop_pose = path->points.at(stop_line_idx).point.pose; + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::UNKNOWN); const double signed_arc_dist_to_stop_point = motion_utils::calcSignedArcLength( path->points, current_pose.pose.position, path->points.at(stop_line_idx).point.pose.position); diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp index 542652cdf604b..1789b6dc6ff3c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp @@ -49,6 +49,7 @@ NoStoppingAreaModule::NoStoppingAreaModule( no_stopping_area_reg_elem_(no_stopping_area_reg_elem), planner_param_(planner_param) { + velocity_factor_.init(VelocityFactor::NO_STOPPING_AREA); state_machine_.setState(StateMachine::State::GO); state_machine_.setMarginTime(planner_param_.state_clear_time); } @@ -193,6 +194,9 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason stop_factor.stop_pose = stop_point->second; stop_factor.stop_factor_points = debug_data_.stuck_points; planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_point->second, + VelocityFactor::UNKNOWN); } // Create legacy StopReason diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index 3ff3694762ae2..9bafd9e707741 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp @@ -63,6 +63,8 @@ OcclusionSpotModule::OcclusionSpotModule( const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), param_(planner_param) { + velocity_factor_.init(VelocityFactor::UNKNOWN); + if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { debug_data_.detection_type = "occupancy"; //! occupancy grid limitation( 100 * 100 ) diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp index 5bb27d1c0b24c..50334e356556b 100644 --- a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp @@ -33,6 +33,8 @@ RunOutModule::RunOutModule( debug_ptr_(debug_ptr), state_machine_(std::make_unique(planner_param.state_param)) { + velocity_factor_.init(VelocityFactor::UNKNOWN); + if (planner_param.run_out.use_partition_lanelet) { const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr(); planning_utils::getAllPartitionLanelets(ll, partition_lanelets_); diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp index 10c42ccd894a4..0210c716ac514 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp @@ -33,6 +33,7 @@ StopLineModule::StopLineModule( stop_line_(stop_line), state_(State::APPROACH) { + velocity_factor_.init(VelocityFactor::STOP_SIGN); planner_param_ = planner_param; } @@ -89,6 +90,8 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop stop_factor.stop_pose = stop_pose; stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::APPROACHING); } // Move to stopped state if stopped @@ -133,6 +136,8 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop stop_factor.stop_pose = ego_pos_on_path.pose; stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::STOPPED); } const auto elapsed_time = (clock_->now() - *stopped_time_).seconds(); diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp index 7ac0d3797e545..523750c833889 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp @@ -50,6 +50,10 @@ void TrafficLightModuleManager::modifyPathVelocity( tl_state.header.stamp = path->header.stamp; tl_state.is_module_running = false; + autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; + velocity_factor_array.header.frame_id = "map"; + velocity_factor_array.header.stamp = clock_->now(); + tier4_planning_msgs::msg::StopReasonArray stop_reason_array; stop_reason_array.header.frame_id = "map"; stop_reason_array.header.stamp = path->header.stamp; @@ -60,9 +64,15 @@ void TrafficLightModuleManager::modifyPathVelocity( tier4_planning_msgs::msg::StopReason stop_reason; std::shared_ptr traffic_light_scene_module( std::dynamic_pointer_cast(scene_module)); + traffic_light_scene_module->resetVelocityFactor(); traffic_light_scene_module->setPlannerData(planner_data_); traffic_light_scene_module->modifyPathVelocity(path, &stop_reason); + // The velocity factor must be called after modifyPathVelocity. + const auto velocity_factor = traffic_light_scene_module->getVelocityFactor(); + if (velocity_factor.type != VelocityFactor::UNKNOWN) { + velocity_factor_array.factors.emplace_back(velocity_factor); + } if (stop_reason.reason != "") { stop_reason_array.stop_reasons.emplace_back(stop_reason); } @@ -91,6 +101,7 @@ void TrafficLightModuleManager::modifyPathVelocity( if (!stop_reason_array.stop_reasons.empty()) { pub_stop_reason_->publish(stop_reason_array); } + pub_velocity_factor_->publish(velocity_factor_array); pub_debug_->publish(debug_marker_array); pub_virtual_wall_->publish(virtual_wall_marker_array); pub_tl_state_->publish(tl_state); diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp index 8620ed5fe7d34..4f09f89a458d3 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp @@ -199,6 +199,7 @@ TrafficLightModule::TrafficLightModule( input_(Input::PERCEPTION), is_prev_state_stop_(false) { + velocity_factor_.init(VelocityFactor::TRAFFIC_SIGNAL); planner_param_ = planner_param; } @@ -487,6 +488,9 @@ autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopP stop_factor.stop_factor_points = std::vector{ debug_data_.highest_confidence_traffic_light_point.value()}; } + velocity_factor_.set( + modified_path.points, planner_data_->current_pose.pose, target_point_with_lane_id.point.pose, + VelocityFactor::UNKNOWN); planning_utils::appendStopReason(stop_factor, stop_reason); return modified_path; diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp index d365de58adcea..263daa9c169bc 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp @@ -194,6 +194,8 @@ VirtualTrafficLightModule::VirtualTrafficLightModule( lane_(lane), planner_param_(planner_param) { + velocity_factor_.init(VelocityFactor::V2I_GATE_CONTROL_ENTER); + // Get map data const auto instrument = reg_elem_.getVirtualTrafficLight(); const auto instrument_bottom_line = toAutowarePoints(instrument); @@ -249,6 +251,7 @@ bool VirtualTrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopRe // Initialize setInfrastructureCommand({}); *stop_reason = planning_utils::initializeStopReason(StopReason::VIRTUAL_TRAFFIC_LIGHT); + module_data_ = {}; // Copy data @@ -575,6 +578,9 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( // Set StopReason setStopReason(stop_pose, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::UNKNOWN, + command_.type); // Set data for visualization module_data_.stop_head_pose_at_stop_line = @@ -607,6 +613,8 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine( // Set StopReason setStopReason(stop_pose, stop_reason); + velocity_factor_.set( + path->points, planner_data_->current_pose.pose, stop_pose, VelocityFactor::UNKNOWN); // Set data for visualization module_data_.stop_head_pose_at_end_line = diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index a6ca4a7a91563..7fe64181ff420 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -21,6 +21,7 @@ #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" +#include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "tier4_planning_msgs/msg/stop_reason_array.hpp" #include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" @@ -32,6 +33,8 @@ #include #include +using autoware_adapi_v1_msgs::msg::VelocityFactor; +using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; @@ -50,6 +53,8 @@ class PlannerInterface { stop_reasons_pub_ = node.create_publisher("~/output/stop_reasons", 1); + velocity_factors_pub_ = + node.create_publisher("/planning/velocity_factors/obstacle_cruise", 1); stop_speed_exceeded_pub_ = node.create_publisher("~/output/stop_speed_exceeded", 1); } @@ -110,6 +115,7 @@ class PlannerInterface // Publishers rclcpp::Publisher::SharedPtr stop_reasons_pub_; + rclcpp::Publisher::SharedPtr velocity_factors_pub_; rclcpp::Publisher::SharedPtr stop_speed_exceeded_pub_; // Vehicle Parameters diff --git a/planning/obstacle_cruise_planner/package.xml b/planning/obstacle_cruise_planner/package.xml index 399fe1a39608b..dd294674bb160 100644 --- a/planning/obstacle_cruise_planner/package.xml +++ b/planning/obstacle_cruise_planner/package.xml @@ -16,6 +16,7 @@ autoware_cmake + autoware_adapi_v1_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs geometry_msgs diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index db2c9c4407ca6..45ed1d3f92d0e 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -72,6 +72,26 @@ tier4_planning_msgs::msg::StopReasonArray makeEmptyStopReasonArray( return stop_reason_array; } +VelocityFactorArray makeVelocityFactorArray( + const rclcpp::Time & time, const std::optional pose = std::nullopt) +{ + VelocityFactorArray velocity_factor_array; + velocity_factor_array.header.frame_id = "map"; + velocity_factor_array.header.stamp = time; + + if (pose) { + using distance_type = VelocityFactor::_distance_type; + VelocityFactor velocity_factor; + velocity_factor.type = VelocityFactor::ROUTE_OBSTACLE; + velocity_factor.pose = pose.value(); + velocity_factor.distance = std::numeric_limits::quiet_NaN(); + velocity_factor.status = VelocityFactor::UNKNOWN; + velocity_factor.detail = std::string(); + velocity_factor_array.factors.push_back(velocity_factor); + } + return velocity_factor_array; +} + double calcMinimumDistanceToStop( const double initial_vel, const double max_acc, const double min_acc) { @@ -92,6 +112,7 @@ Trajectory PlannerInterface::generateStopTrajectory( if (planner_data.target_obstacles.empty()) { stop_reasons_pub_->publish(makeEmptyStopReasonArray(planner_data.current_time)); + velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time)); // delete marker const auto markers = @@ -207,6 +228,7 @@ Trajectory PlannerInterface::generateStopTrajectory( const auto stop_reasons_msg = makeStopReasonArray(planner_data.current_time, stop_pose, *closest_stop_obstacle); stop_reasons_pub_->publish(stop_reasons_msg); + velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose)); // Publish if ego vehicle collides with the obstacle with a limit acceleration const auto stop_speed_exceeded_msg = diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp index 2b34a92c4c747..2d3dc40914895 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/debug_marker.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -43,6 +44,8 @@ using std_msgs::msg::Header; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; +using autoware_adapi_v1_msgs::msg::VelocityFactor; +using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; @@ -113,6 +116,7 @@ class ObstacleStopPlannerDebugNode MarkerArray makeVirtualWallMarker(); MarkerArray makeVisualizationMarker(); StopReasonArray makeStopReasonArray(); + VelocityFactorArray makeVelocityFactorArray(); void setDebugValues(const DebugValues::TYPE type, const double val) { @@ -124,6 +128,7 @@ class ObstacleStopPlannerDebugNode rclcpp::Publisher::SharedPtr virtual_wall_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; rclcpp::Publisher::SharedPtr stop_reason_pub_; + rclcpp::Publisher::SharedPtr velocity_factor_pub_; rclcpp::Publisher::SharedPtr pub_debug_values_; rclcpp::Node * node_; double base_link2front_; diff --git a/planning/obstacle_stop_planner/package.xml b/planning/obstacle_stop_planner/package.xml index eb03e76b520ce..5b48e16a848d3 100644 --- a/planning/obstacle_stop_planner/package.xml +++ b/planning/obstacle_stop_planner/package.xml @@ -19,6 +19,7 @@ autoware_cmake + autoware_adapi_v1_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs diagnostic_msgs diff --git a/planning/obstacle_stop_planner/src/debug_marker.cpp b/planning/obstacle_stop_planner/src/debug_marker.cpp index 813c4c3b9c7dc..683bbdf9e6113 100644 --- a/planning/obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/obstacle_stop_planner/src/debug_marker.cpp @@ -46,6 +46,8 @@ ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode( virtual_wall_pub_ = node_->create_publisher("~/virtual_wall", 1); 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/obstacle_stop", 1); pub_debug_values_ = node_->create_publisher("~/obstacle_stop/debug_values", 1); } @@ -149,6 +151,8 @@ void ObstacleStopPlannerDebugNode::publish() /* 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); // publish debug values Float32MultiArrayStamped debug_msg{}; @@ -389,4 +393,23 @@ StopReasonArray ObstacleStopPlannerDebugNode::makeStopReasonArray() return stop_reason_array; } +VelocityFactorArray ObstacleStopPlannerDebugNode::makeVelocityFactorArray() +{ + VelocityFactorArray velocity_factor_array; + velocity_factor_array.header.frame_id = "map"; + velocity_factor_array.header.stamp = node_->now(); + + if (stop_pose_ptr_) { + using distance_type = VelocityFactor::_distance_type; + VelocityFactor velocity_factor; + velocity_factor.type = VelocityFactor::ROUTE_OBSTACLE; + velocity_factor.pose = *stop_pose_ptr_; + velocity_factor.distance = std::numeric_limits::quiet_NaN(); + velocity_factor.status = VelocityFactor::UNKNOWN; + velocity_factor.detail = std::string(); + velocity_factor_array.factors.push_back(velocity_factor); + } + return velocity_factor_array; +} + } // namespace motion_planning diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp index 36e1e22f4aa23..bd0fe48bbda28 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp @@ -17,6 +17,7 @@ #include +#include #include #include #include @@ -31,6 +32,8 @@ namespace surround_obstacle_checker { +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; @@ -63,6 +66,7 @@ class SurroundObstacleCheckerDebugNode rclcpp::Publisher::SharedPtr debug_virtual_wall_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; rclcpp::Publisher::SharedPtr stop_reason_pub_; + rclcpp::Publisher::SharedPtr velocity_factor_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_offset_pub_; @@ -77,6 +81,7 @@ class SurroundObstacleCheckerDebugNode MarkerArray makeVirtualWallMarker(); MarkerArray makeVisualizationMarker(); StopReasonArray makeStopReasonArray(); + VelocityFactorArray makeVelocityFactorArray(); Polygon2d createSelfPolygonWithOffset(const Polygon2d & base_polygon, const double & offset); PolygonStamped boostPolygonToPolygonStamped(const Polygon2d & boost_polygon, const double & z); diff --git a/planning/surround_obstacle_checker/package.xml b/planning/surround_obstacle_checker/package.xml index 243cf2cf3dfd1..e1d31acbaba1c 100644 --- a/planning/surround_obstacle_checker/package.xml +++ b/planning/surround_obstacle_checker/package.xml @@ -14,6 +14,7 @@ autoware_cmake + autoware_adapi_v1_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_tf2 diff --git a/planning/surround_obstacle_checker/src/debug_marker.cpp b/planning/surround_obstacle_checker/src/debug_marker.cpp index 1fe52d4273571..c30f778584fd7 100644 --- a/planning/surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/surround_obstacle_checker/src/debug_marker.cpp @@ -51,6 +51,8 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( node.create_publisher("~/virtual_wall", 1); 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); vehicle_footprint_offset_pub_ = node.create_publisher("~/debug/footprint_offset", 1); @@ -116,6 +118,8 @@ void SurroundObstacleCheckerDebugNode::publish() /* 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); /* reset variables */ stop_pose_ptr_ = nullptr; @@ -183,6 +187,25 @@ StopReasonArray SurroundObstacleCheckerDebugNode::makeStopReasonArray() return stop_reason_array; } +VelocityFactorArray SurroundObstacleCheckerDebugNode::makeVelocityFactorArray() +{ + VelocityFactorArray velocity_factor_array; + velocity_factor_array.header.frame_id = "map"; + velocity_factor_array.header.stamp = clock_->now(); + + if (stop_pose_ptr_) { + using distance_type = VelocityFactor::_distance_type; + VelocityFactor velocity_factor; + velocity_factor.type = VelocityFactor::SURROUNDING_OBSTACLE; + velocity_factor.pose = *stop_pose_ptr_; + velocity_factor.distance = std::numeric_limits::quiet_NaN(); + velocity_factor.status = VelocityFactor::UNKNOWN; + velocity_factor.detail = std::string(); + velocity_factor_array.factors.push_back(velocity_factor); + } + return velocity_factor_array; +} + Polygon2d SurroundObstacleCheckerDebugNode::createSelfPolygonWithOffset( const Polygon2d & base_polygon, const double & offset) {