feat(external_velocity_limit_selector): publish string for debug

Signed-off-by: satoshi-ota <[email protected]>
autowarefoundation · Dec 27, 2022 · cece48c · cece48c
1 parent 47181fc
commit cece48c
Show file tree

Hide file tree

Showing 3 changed files with 42 additions and 2 deletions.
diff --git a/...lector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp b/...lector/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_
diff --git a/planning/external_velocity_limit_selector/package.xml b/planning/external_velocity_limit_selector/package.xml
@@ -15,6 +15,7 @@
 
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>tier4_debug_msgs</depend>
   <depend>tier4_planning_msgs</depend>
 
   <exec_depend>ros2cli</exec_depend>

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
@@ -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(
@@ -89,6 +106,8 @@ ExternalVelocityLimitSelectorNode::ExternalVelocityLimitSelectorNode(
   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)
 {