Merge branch 'main' into RT1-8205-implement-terminal-lane-change-feature

Author: mkquda

common/autoware_adapi_specs/include/autoware/adapi_specs/vehicle.hpp

@@ -41,7 +41,7 @@ struct VehicleStatus
   using Message = autoware_adapi_v1_msgs::msg::VehicleStatus;
   static constexpr char name[] = "/api/vehicle/status";
   static constexpr size_t depth = 1;
-  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
   static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
 };
 

common/autoware_universe_utils/CMakeLists.txt

@@ -21,6 +21,7 @@ ament_auto_add_library(autoware_universe_utils SHARED
   src/geometry/sat_2d.cpp
   src/math/sin_table.cpp
   src/math/trigonometry.cpp
+  src/ros/diagnostics_interface.cpp
   src/ros/msg_operation.cpp
   src/ros/marker_helper.cpp
   src/ros/logger_level_configure.cpp

common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_traits.hpp

@@ -15,6 +15,16 @@
 #ifndef AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_TRAITS_HPP_
 #define AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_TRAITS_HPP_
 
+#include <autoware_internal_debug_msgs/msg/bool_stamped.hpp>
+#include <autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp>
+#include <autoware_internal_debug_msgs/msg/float32_stamped.hpp>
+#include <autoware_internal_debug_msgs/msg/float64_multi_array_stamped.hpp>
+#include <autoware_internal_debug_msgs/msg/float64_stamped.hpp>
+#include <autoware_internal_debug_msgs/msg/int32_multi_array_stamped.hpp>
+#include <autoware_internal_debug_msgs/msg/int32_stamped.hpp>
+#include <autoware_internal_debug_msgs/msg/int64_multi_array_stamped.hpp>
+#include <autoware_internal_debug_msgs/msg/int64_stamped.hpp>
+#include <autoware_internal_debug_msgs/msg/string_stamped.hpp>
 #include <tier4_debug_msgs/msg/bool_stamped.hpp>
 #include <tier4_debug_msgs/msg/float32_multi_array_stamped.hpp>
 #include <tier4_debug_msgs/msg/float32_stamped.hpp>
@@ -84,6 +94,58 @@ template <>
 struct is_debug_message<tier4_debug_msgs::msg::StringStamped> : std::true_type
 {
 };
+
+template <>
+struct is_debug_message<autoware_internal_debug_msgs::msg::BoolStamped> : std::true_type
+{
+};
+
+template <>
+struct is_debug_message<autoware_internal_debug_msgs::msg::Float32MultiArrayStamped>
+: std::true_type
+{
+};
+
+template <>
+struct is_debug_message<autoware_internal_debug_msgs::msg::Float32Stamped> : std::true_type
+{
+};
+
+template <>
+struct is_debug_message<autoware_internal_debug_msgs::msg::Float64MultiArrayStamped>
+: std::true_type
+{
+};
+
+template <>
+struct is_debug_message<autoware_internal_debug_msgs::msg::Float64Stamped> : std::true_type
+{
+};
+
+template <>
+struct is_debug_message<autoware_internal_debug_msgs::msg::Int32MultiArrayStamped> : std::true_type
+{
+};
+
+template <>
+struct is_debug_message<autoware_internal_debug_msgs::msg::Int32Stamped> : std::true_type
+{
+};
+
+template <>
+struct is_debug_message<autoware_internal_debug_msgs::msg::Int64MultiArrayStamped> : std::true_type
+{
+};
+
+template <>
+struct is_debug_message<autoware_internal_debug_msgs::msg::Int64Stamped> : std::true_type
+{
+};
+
+template <>
+struct is_debug_message<autoware_internal_debug_msgs::msg::StringStamped> : std::true_type
+{
+};
 }  // namespace autoware::universe_utils::debug_traits
 
 #endif  // AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_TRAITS_HPP_

localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_
-#define AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_
+#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__DIAGNOSTICS_INTERFACE_HPP_
+#define AUTOWARE__UNIVERSE_UTILS__ROS__DIAGNOSTICS_INTERFACE_HPP_
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -22,16 +22,18 @@
 #include <string>
 #include <vector>
 
-namespace autoware::localization_util
+namespace autoware::universe_utils
 {
-class DiagnosticsModule
+class DiagnosticInterface
 {
 public:
-  DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name);
+  DiagnosticInterface(rclcpp::Node * node, const std::string & diagnostic_name);
   void clear();
   void add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg);
   template <typename T>
   void add_key_value(const std::string & key, const T & value);
+  void add_key_value(const std::string & key, const std::string & value);
+  void add_key_value(const std::string & key, bool value);
   void update_level_and_message(const int8_t level, const std::string & message);
   void publish(const rclcpp::Time & publish_time_stamp);
 
@@ -46,19 +48,14 @@ class DiagnosticsModule
 };
 
 template <typename T>
-void DiagnosticsModule::add_key_value(const std::string & key, const T & value)
+void DiagnosticInterface::add_key_value(const std::string & key, const T & value)
 {
   diagnostic_msgs::msg::KeyValue key_value;
   key_value.key = key;
   key_value.value = std::to_string(value);
   add_key_value(key_value);
 }
 
-template <>
-void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value);
-template <>
-void DiagnosticsModule::add_key_value(const std::string & key, const bool & value);
+}  // namespace autoware::universe_utils
 
-}  // namespace autoware::localization_util
-
-#endif  // AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_
+#endif  // AUTOWARE__UNIVERSE_UTILS__ROS__DIAGNOSTICS_INTERFACE_HPP_

common/autoware_universe_utils/package.xml

@@ -12,6 +12,7 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
+  <depend>autoware_internal_debug_msgs</depend>
   <depend>autoware_internal_msgs</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_planning_msgs</depend>

localization/autoware_localization_util/src/diagnostics_module.cpp common/autoware_universe_utils/src/ros/diagnostics_interface.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "autoware/localization_util/diagnostics_module.hpp"
+#include "autoware/universe_utils/ros/diagnostics_interface.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -21,9 +21,9 @@
 #include <algorithm>
 #include <string>
 
-namespace autoware::localization_util
+namespace autoware::universe_utils
 {
-DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name)
+DiagnosticInterface::DiagnosticInterface(rclcpp::Node * node, const std::string & diagnostic_name)
 : clock_(node->get_clock())
 {
   diagnostics_pub_ =
@@ -34,7 +34,7 @@ DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & di
   diagnostics_status_msg_.hardware_id = node->get_name();
 }
 
-void DiagnosticsModule::clear()
+void DiagnosticInterface::clear()
 {
   diagnostics_status_msg_.values.clear();
   diagnostics_status_msg_.values.shrink_to_fit();
@@ -43,7 +43,7 @@ void DiagnosticsModule::clear()
   diagnostics_status_msg_.message = "";
 }
 
-void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg)
+void DiagnosticInterface::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg)
 {
   auto it = std::find_if(
     std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values),
@@ -56,25 +56,23 @@ void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key
   }
 }
 
-template <>
-void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value)
+void DiagnosticInterface::add_key_value(const std::string & key, const std::string & value)
 {
   diagnostic_msgs::msg::KeyValue key_value;
   key_value.key = key;
   key_value.value = value;
   add_key_value(key_value);
 }
 
-template <>
-void DiagnosticsModule::add_key_value(const std::string & key, const bool & value)
+void DiagnosticInterface::add_key_value(const std::string & key, bool value)
 {
   diagnostic_msgs::msg::KeyValue key_value;
   key_value.key = key;
   key_value.value = value ? "True" : "False";
   add_key_value(key_value);
 }
 
-void DiagnosticsModule::update_level_and_message(const int8_t level, const std::string & message)
+void DiagnosticInterface::update_level_and_message(const int8_t level, const std::string & message)
 {
   if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) {
     if (!diagnostics_status_msg_.message.empty()) {
@@ -87,12 +85,12 @@ void DiagnosticsModule::update_level_and_message(const int8_t level, const std::
   }
 }
 
-void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp)
+void DiagnosticInterface::publish(const rclcpp::Time & publish_time_stamp)
 {
   diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp));
 }
 
-diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_array(
+diagnostic_msgs::msg::DiagnosticArray DiagnosticInterface::create_diagnostics_array(
   const rclcpp::Time & publish_time_stamp) const
 {
   diagnostic_msgs::msg::DiagnosticArray diagnostics_msg;
@@ -105,4 +103,4 @@ diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_arra
 
   return diagnostics_msg;
 }
-}  // namespace autoware::localization_util
+}  // namespace autoware::universe_utils