fix(external_velocity_limit_selector): increase history depth externa…

…l velocity limit selector (autowarefoundation#2581)

* fix(external_velocity_limit_selector): increase history depth

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(external_velocity_limit_selector): publish string for debug

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

* feat(external_velocity_limit_selector): remap debug topic

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp

@@ -17,6 +17,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <tier4_debug_msgs/msg/string_stamped.hpp>
 #include <tier4_planning_msgs/msg/velocity_limit.hpp>
 #include <tier4_planning_msgs/msg/velocity_limit_clear_command.hpp>
 
@@ -25,10 +26,13 @@
 #include <string>
 #include <unordered_map>
 
+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<std::string, VelocityLimit>;
+
 class ExternalVelocityLimitSelectorNode : public rclcpp::Node
 {
 public:
@@ -58,18 +62,20 @@ class ExternalVelocityLimitSelectorNode : public rclcpp::Node
   rclcpp::Subscription<VelocityLimit>::SharedPtr sub_external_velocity_limit_from_internal_;
   rclcpp::Subscription<VelocityLimitClearCommand>::SharedPtr sub_velocity_limit_clear_command_;
   rclcpp::Publisher<VelocityLimit>::SharedPtr pub_external_velocity_limit_;
+  rclcpp::Publisher<StringStamped>::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<std::string, VelocityLimit> velocity_limit_table_;
+  VelocityLimitTable velocity_limit_table_;
 };
 
 #endif  // EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_

planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml

@@ -8,6 +8,7 @@
   <arg name="input_velocity_limit_from_internal" default="/planning/scenario_planning/max_velocity_candidates"/>
   <arg name="input_velocity_limit_clear_command_from_internal" default="/planning/scenario_planning/clear_velocity_limit"/>
   <arg name="output_velocity_limit_from_selector" default="/planning/scenario_planning/max_velocity"/>
+  <arg name="output_debug_string" default="/planning/scenario_planning/external_velocity_limit_selector/debug"/>
 
   <node pkg="external_velocity_limit_selector" exec="external_velocity_limit_selector" name="external_velocity_limit_selector" output="screen">
     <param from="$(var common_param_path)"/>
@@ -16,5 +17,6 @@
     <remap from="input/velocity_limit_from_internal" to="$(var input_velocity_limit_from_internal)"/>
     <remap from="input/velocity_limit_clear_command_from_internal" to="$(var input_velocity_limit_clear_command_from_internal)"/>
     <remap from="output/external_velocity_limit" to="$(var output_velocity_limit_from_selector)"/>
+    <remap from="output/debug" to="$(var output_debug_string)"/>
   </node>
 </launch>

planning/external_velocity_limit_selector/package.xml

@@ -20,6 +20,7 @@
 
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>tier4_debug_msgs</depend>
   <depend>tier4_planning_msgs</depend>
 
   <exec_depend>ros2cli</exec_depend>

planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp

@@ -24,7 +24,7 @@
 namespace
 {
 VelocityLimit getHardestLimit(
-  const std::unordered_map<std::string, VelocityLimit> & 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<VelocityLimit>(
-    "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<VelocityLimitClearCommand>(
-    "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<VelocityLimit>("output/external_velocity_limit", 1);
 
+  pub_debug_string_ = this->create_publisher<StringStamped>("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,13 +158,23 @@ void ExternalVelocityLimitSelectorNode::onVelocityLimitClearCommand(
 
   const auto velocity_limit = getCurrentVelocityLimit();
   publishVelocityLimit(velocity_limit);
+
+  publishDebugString();
 }
 
 void ExternalVelocityLimitSelectorNode::publishVelocityLimit(const VelocityLimit & velocity_limit)
 {
   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)
 {