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..7595533a68d7c 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 @@ -446,7 +446,7 @@ boost::optional CrosswalkModule::findNearestStopPoint const auto stop_line_distance = exist_stopline_in_map ? 0.0 : planner_param_.stop_line_distance; const auto margin = stop_at_stop_line ? stop_line_distance + base_link2front : planner_param_.stop_margin + base_link2front; - const auto stop_pose = calcLongitudinalOffsetPose(sparse_resample_path.points, p_stop, -margin); + const auto stop_pose = calcLongitudinalOffsetPose(ego_path.points, p_stop, -margin); if (!stop_pose) { return {}; diff --git a/planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp b/planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp index 0786e592fe363..93fad135ac678 100644 --- a/planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp +++ b/planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp @@ -17,6 +17,7 @@ #include +#include #include #include @@ -25,10 +26,13 @@ #include #include +using tier4_debug_msgs::msg::StringStamped; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; using tier4_planning_msgs::msg::VelocityLimitConstraints; +using VelocityLimitTable = std::unordered_map; + class ExternalVelocityLimitSelectorNode : public rclcpp::Node { public: @@ -58,18 +62,20 @@ class ExternalVelocityLimitSelectorNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_external_velocity_limit_from_internal_; rclcpp::Subscription::SharedPtr sub_velocity_limit_clear_command_; rclcpp::Publisher::SharedPtr pub_external_velocity_limit_; + rclcpp::Publisher::SharedPtr pub_debug_string_; void publishVelocityLimit(const VelocityLimit & velocity_limit); void setVelocityLimitFromAPI(const VelocityLimit & velocity_limit); void setVelocityLimitFromInternal(const VelocityLimit & velocity_limit); void clearVelocityLimit(const std::string & sender); void updateVelocityLimit(); + void publishDebugString(); VelocityLimit getCurrentVelocityLimit() { return hardest_limit_; } // Parameters NodeParam node_param_{}; VelocityLimit hardest_limit_{}; - std::unordered_map velocity_limit_table_; + VelocityLimitTable velocity_limit_table_; }; #endif // EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ diff --git a/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml b/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml index 9cc9141b09807..5ef089f3d3ee7 100644 --- a/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml +++ b/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml @@ -8,6 +8,7 @@ + @@ -16,5 +17,6 @@ + diff --git a/planning/external_velocity_limit_selector/package.xml b/planning/external_velocity_limit_selector/package.xml index 2a0d2ff51680c..8142793898506 100644 --- a/planning/external_velocity_limit_selector/package.xml +++ b/planning/external_velocity_limit_selector/package.xml @@ -15,6 +15,7 @@ rclcpp rclcpp_components + tier4_debug_msgs tier4_planning_msgs ros2cli diff --git a/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp b/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp index b4c3d0f5e7758..bda1370545edc 100644 --- a/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp +++ b/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp @@ -24,7 +24,7 @@ namespace { VelocityLimit getHardestLimit( - const std::unordered_map & velocity_limits, + const VelocityLimitTable & velocity_limits, const ExternalVelocityLimitSelectorNode::NodeParam & node_param) { VelocityLimit hardest_limit{}; @@ -65,6 +65,23 @@ VelocityLimit getHardestLimit( return hardest_limit; } + +std::string getDebugString(const VelocityLimitTable & velocity_limits) +{ + std::ostringstream string_stream; + string_stream << std::boolalpha << std::fixed << std::setprecision(2); + for (const auto & limit : velocity_limits) { + string_stream << "[" << limit.first << "]"; + string_stream << "("; + string_stream << limit.second.use_constraints << ","; + string_stream << limit.second.max_velocity << ","; + string_stream << limit.second.constraints.min_acceleration << ","; + string_stream << limit.second.constraints.min_jerk << ","; + string_stream << limit.second.constraints.max_jerk << ")"; + } + + return string_stream.str(); +} } // namespace ExternalVelocityLimitSelectorNode::ExternalVelocityLimitSelectorNode( @@ -78,17 +95,19 @@ ExternalVelocityLimitSelectorNode::ExternalVelocityLimitSelectorNode( std::bind(&ExternalVelocityLimitSelectorNode::onVelocityLimitFromAPI, this, _1)); sub_external_velocity_limit_from_internal_ = this->create_subscription( - "input/velocity_limit_from_internal", rclcpp::QoS{1}.transient_local(), + "input/velocity_limit_from_internal", rclcpp::QoS{10}.transient_local(), std::bind(&ExternalVelocityLimitSelectorNode::onVelocityLimitFromInternal, this, _1)); sub_velocity_limit_clear_command_ = this->create_subscription( - "input/velocity_limit_clear_command_from_internal", rclcpp::QoS{1}.transient_local(), + "input/velocity_limit_clear_command_from_internal", rclcpp::QoS{10}.transient_local(), std::bind(&ExternalVelocityLimitSelectorNode::onVelocityLimitClearCommand, this, _1)); // Output pub_external_velocity_limit_ = this->create_publisher("output/external_velocity_limit", 1); + pub_debug_string_ = this->create_publisher("output/debug", 1); + // Params { auto & p = node_param_; @@ -112,6 +131,8 @@ void ExternalVelocityLimitSelectorNode::onVelocityLimitFromAPI( const auto velocity_limit = getCurrentVelocityLimit(); publishVelocityLimit(velocity_limit); + + publishDebugString(); } void ExternalVelocityLimitSelectorNode::onVelocityLimitFromInternal( @@ -122,6 +143,8 @@ void ExternalVelocityLimitSelectorNode::onVelocityLimitFromInternal( const auto velocity_limit = getCurrentVelocityLimit(); publishVelocityLimit(velocity_limit); + + publishDebugString(); } void ExternalVelocityLimitSelectorNode::onVelocityLimitClearCommand( @@ -135,6 +158,8 @@ void ExternalVelocityLimitSelectorNode::onVelocityLimitClearCommand( const auto velocity_limit = getCurrentVelocityLimit(); publishVelocityLimit(velocity_limit); + + publishDebugString(); } void ExternalVelocityLimitSelectorNode::publishVelocityLimit(const VelocityLimit & velocity_limit) @@ -142,6 +167,14 @@ void ExternalVelocityLimitSelectorNode::publishVelocityLimit(const VelocityLimit pub_external_velocity_limit_->publish(velocity_limit); } +void ExternalVelocityLimitSelectorNode::publishDebugString() +{ + StringStamped debug_string{}; + debug_string.stamp = this->now(); + debug_string.data = getDebugString(velocity_limit_table_); + pub_debug_string_->publish(debug_string); +} + void ExternalVelocityLimitSelectorNode::setVelocityLimitFromAPI( const VelocityLimit & velocity_limit) {