merge branch main into autoware_msg

Signed-off-by: liu cui <[email protected]>

.github/CODEOWNERS

@@ -21,35 +21,25 @@ common/goal_distance_calculator/** [email protected]
 common/grid_map_utils/** [email protected]
 common/interpolation/** [email protected] [email protected]
 common/kalman_filter/** [email protected] [email protected] [email protected]
-common/mission_planner_rviz_plugin/** [email protected]
 common/motion_utils/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 common/object_recognition_utils/** [email protected] [email protected] [email protected] [email protected]
 common/osqp_interface/** [email protected] [email protected] [email protected] [email protected]
 common/path_distance_calculator/** [email protected]
 common/perception_utils/** [email protected] [email protected] [email protected]
 common/polar_grid/** [email protected]
 common/qp_interface/** [email protected] [email protected] [email protected] [email protected]
-common/rtc_manager_rviz_plugin/** [email protected] [email protected]
 common/signal_processing/** [email protected] [email protected] [email protected] [email protected] [email protected]
 common/tensorrt_common/** [email protected] [email protected]
 common/tier4_adapi_rviz_plugin/** [email protected] [email protected] [email protected]
 common/tier4_api_utils/** [email protected]
-common/tier4_automatic_goal_rviz_plugin/** [email protected] [email protected] [email protected] [email protected]
 common/tier4_autoware_utils/** [email protected] [email protected] [email protected]
-common/tier4_calibration_rviz_plugin/** [email protected]
 common/tier4_camera_view_rviz_plugin/** [email protected] [email protected]
-common/tier4_control_rviz_plugin/** [email protected]
 common/tier4_datetime_rviz_plugin/** [email protected]
-common/tier4_debug_rviz_plugin/** [email protected]
 common/tier4_localization_rviz_plugin/** [email protected] [email protected]
-common/tier4_logging_level_configure_rviz_plugin/** [email protected] [email protected] [email protected]
 common/tier4_perception_rviz_plugin/** [email protected]
 common/tier4_planning_rviz_plugin/** [email protected] [email protected]
-common/tier4_screen_capture_rviz_plugin/** [email protected] [email protected] [email protected]
-common/tier4_simulated_clock_rviz_plugin/** [email protected]
-common/tier4_state_rviz_plugin/** [email protected] [email protected]
+common/tier4_state_rviz_plugin/** [email protected] [email protected] [email protected]
 common/tier4_system_rviz_plugin/** [email protected]
-common/tier4_target_object_type_rviz_plugin/** [email protected]
 common/tier4_traffic_light_rviz_plugin/** [email protected]
 common/tier4_vehicle_rviz_plugin/** [email protected]
 common/time_utils/** [email protected] [email protected] [email protected]
@@ -156,13 +146,14 @@ perception/traffic_light_map_based_detector/** [email protected] tao.zhong
 perception/traffic_light_multi_camera_fusion/** [email protected] [email protected]
 perception/traffic_light_occlusion_predictor/** [email protected] [email protected]
 perception/traffic_light_visualization/** [email protected] [email protected]
+planning/autoware_behavior_path_external_request_lane_change_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
+planning/autoware_behavior_velocity_planner/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/autoware_planning_test_manager/** [email protected] [email protected]
 planning/autoware_remaining_distance_time_calculator/** [email protected]
 planning/autoware_static_centerline_generator/** [email protected] [email protected]
 planning/behavior_path_avoidance_by_lane_change_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_path_avoidance_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_path_dynamic_avoidance_module/** [email protected] [email protected] [email protected] [email protected]
-planning/behavior_path_external_request_lane_change_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_path_goal_planner_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_path_lane_change_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_path_planner/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
@@ -179,7 +170,6 @@ planning/behavior_velocity_no_drivable_lane_module/** [email protected]
 planning/behavior_velocity_no_stopping_area_module/** [email protected] [email protected] [email protected]
 planning/behavior_velocity_occlusion_spot_module/** [email protected] [email protected] [email protected]
 planning/behavior_velocity_out_of_lane_module/** [email protected] [email protected] [email protected] [email protected]
-planning/behavior_velocity_planner/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_velocity_planner_common/** [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_velocity_run_out_module/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 planning/behavior_velocity_speed_bump_module/** [email protected] [email protected] [email protected]

common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp

@@ -0,0 +1,36 @@
+// Copyright 2024 The Autoware Contributors
+//
+// 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 AUTOWARE_AD_API_SPECS__SYSTEM_HPP_
+#define AUTOWARE_AD_API_SPECS__SYSTEM_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/heartbeat.hpp>
+
+namespace autoware_ad_api::system
+{
+
+struct Heartbeat
+{
+  using Message = autoware_adapi_v1_msgs::msg::Heartbeat;
+  static constexpr char name[] = "/api/system/heartbeat";
+  static constexpr size_t depth = 10;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
+}  // namespace autoware_ad_api::system
+
+#endif  // AUTOWARE_AD_API_SPECS__SYSTEM_HPP_

common/motion_utils/include/motion_utils/trajectory/trajectory.hpp

@@ -1903,20 +1903,21 @@ extern template void insertOrientation<std::vector<autoware_planning_msgs::msg::
  * radians (default: M_PI_2)
  */
 template <class T>
-void removeInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2)
+void removeFirstInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2)
 {
-  for (size_t i = 1; i < points.size();) {
-    const auto p1 = tier4_autoware_utils::getPose(points.at(i - 1));
-    const auto p2 = tier4_autoware_utils::getPose(points.at(i));
+  for (auto itr = points.begin(); std::next(itr) != points.end();) {
+    const auto p1 = tier4_autoware_utils::getPose(*itr);
+    const auto p2 = tier4_autoware_utils::getPose(*std::next(itr));
     const double yaw1 = tf2::getYaw(p1.orientation);
     const double yaw2 = tf2::getYaw(p2.orientation);
 
     if (
       max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2)) ||
       !tier4_autoware_utils::isDrivingForward(p1, p2)) {
-      points.erase(points.begin() + i);
+      points.erase(std::next(itr));
+      return;
     } else {
-      ++i;
+      itr++;
     }
   }
 }

common/motion_utils/test/src/trajectory/test_trajectory.cpp

@@ -5337,10 +5337,10 @@ TEST(trajectory, cropPoints)
   }
 }
 
-TEST(Trajectory, removeInvalidOrientationPoints)
+TEST(Trajectory, removeFirstInvalidOrientationPoints)
 {
   using motion_utils::insertOrientation;
-  using motion_utils::removeInvalidOrientationPoints;
+  using motion_utils::removeFirstInvalidOrientationPoints;
 
   const double max_yaw_diff = M_PI_2;
 
@@ -5351,7 +5351,7 @@ TEST(Trajectory, removeInvalidOrientationPoints)
     auto modified_traj = traj;
     insertOrientation(modified_traj.points, true);
     modifyTrajectory(modified_traj);
-    removeInvalidOrientationPoints(modified_traj.points, max_yaw_diff);
+    removeFirstInvalidOrientationPoints(modified_traj.points, max_yaw_diff);
     EXPECT_EQ(modified_traj.points.size(), expected_size);
     for (size_t i = 0; i < modified_traj.points.size() - 1; ++i) {
       EXPECT_EQ(traj.points.at(i), modified_traj.points.at(i));
@@ -5374,7 +5374,7 @@ TEST(Trajectory, removeInvalidOrientationPoints)
     [&](Trajectory & t) {
       auto invalid_point = t.points.back();
       invalid_point.pose.orientation =
-        tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2));
+        tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_4));
       t.points.push_back(invalid_point);
     },
     traj.points.size());
@@ -5385,21 +5385,10 @@ TEST(Trajectory, removeInvalidOrientationPoints)
     [&](Trajectory & t) {
       auto invalid_point = t.points[4];
       invalid_point.pose.orientation =
-        tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2));
+        tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_4));
       t.points.insert(t.points.begin() + 4, invalid_point);
     },
     traj.points.size());
-
-  // invalid point at the beginning
-  testRemoveInvalidOrientationPoints(
-    traj,
-    [&](Trajectory & t) {
-      auto invalid_point = t.points.front();
-      invalid_point.pose.orientation =
-        tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2));
-      t.points.insert(t.points.begin(), invalid_point);
-    },
-    1);  // expected size is 1 since only the first point remains
 }
 
 TEST(trajectory, calcYawDeviation)

common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp

@@ -15,13 +15,17 @@
 #ifndef TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
 #define TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
 
+#include <utility>
+
 namespace tier4_autoware_utils
 {
 
 float sin(float radian);
 
 float cos(float radian);
 
+std::pair<float, float> sin_and_cos(float radian);
+
 }  // namespace tier4_autoware_utils
 
 #endif  // TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_

common/tier4_autoware_utils/src/math/trigonometry.cpp

@@ -49,4 +49,27 @@ float cos(float radian)
   return sin(radian + static_cast<float>(tier4_autoware_utils::pi) / 2.f);
 }
 
+std::pair<float, float> sin_and_cos(float radian)
+{
+  constexpr float tmp =
+    (180.f / static_cast<float>(tier4_autoware_utils::pi)) * (discrete_arcs_num_360 / 360.f);
+  const float degree = radian * tmp;
+  size_t idx =
+    (static_cast<int>(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) %
+    discrete_arcs_num_360;
+
+  if (idx < discrete_arcs_num_90) {
+    return {g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]};
+  } else if (discrete_arcs_num_90 <= idx && idx < 2 * discrete_arcs_num_90) {
+    idx = 2 * discrete_arcs_num_90 - idx;
+    return {g_sin_table[idx], -g_sin_table[discrete_arcs_num_90 - idx]};
+  } else if (2 * discrete_arcs_num_90 <= idx && idx < 3 * discrete_arcs_num_90) {
+    idx = idx - 2 * discrete_arcs_num_90;
+    return {-g_sin_table[idx], -g_sin_table[discrete_arcs_num_90 - idx]};
+  } else {  // 3 * discrete_arcs_num_90 <= idx && idx < 4 * discrete_arcs_num_90
+    idx = 4 * discrete_arcs_num_90 - idx;
+    return {-g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]};
+  }
+}
+
 }  // namespace tier4_autoware_utils

common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp

@@ -40,3 +40,13 @@ TEST(trigonometry, cos)
         tier4_autoware_utils::cos(x * static_cast<float>(i))) < 10e-5);
   }
 }
+
+TEST(trigonometry, sin_and_cos)
+{
+  float x = 4.f * tier4_autoware_utils::pi / 128.f;
+  for (int i = 0; i < 128; i++) {
+    const auto sin_and_cos = tier4_autoware_utils::sin_and_cos(x * static_cast<float>(i));
+    EXPECT_TRUE(std::abs(std::sin(x * static_cast<float>(i)) - sin_and_cos.first) < 10e-7);
+    EXPECT_TRUE(std::abs(std::cos(x * static_cast<float>(i)) - sin_and_cos.second) < 10e-7);
+  }
+}

common/tier4_state_rviz_plugin/CMakeLists.txt

@@ -13,15 +13,42 @@ add_definitions(-DQT_NO_KEYWORDS)
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/autoware_state_panel.cpp
   src/velocity_steering_factors_panel.cpp
+  src/custom_toggle_switch.cpp
+  src/custom_slider.cpp
+  src/custom_container.cpp
+  src/custom_button.cpp
+  src/custom_icon_label.cpp
+  src/custom_segmented_button.cpp
+  src/custom_segmented_button_item.cpp
+  src/custom_label.cpp
+  src/include/material_colors.hpp
+  src/include/autoware_state_panel.hpp
+  src/include/custom_button.hpp
+  src/include/custom_container.hpp
+  src/include/custom_icon_label.hpp
+  src/include/custom_label.hpp
+  src/include/custom_segmented_button_item.hpp
+  src/include/custom_segmented_button.hpp
+  src/include/custom_slider.hpp
+  src/include/custom_toggle_switch.hpp
+  src/include/velocity_steering_factors_panel.hpp
 )
 
+target_include_directories(
+  ${PROJECT_NAME} PUBLIC
+)
+
+
 target_link_libraries(${PROJECT_NAME}
   ${QT_LIBRARIES}
 )
 
 # Export the plugin to be imported by rviz2
 pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
 
+
+
+
 ament_auto_package(
   INSTALL_TO_SHARE
   icons

common/tier4_state_rviz_plugin/package.xml

@@ -6,6 +6,7 @@
   <description>The autoware state rviz plugin package</description>
   <maintainer email="[email protected]">Hiroki OTA</maintainer>
   <maintainer email="[email protected]">Takagi, Isamu</maintainer>
+  <maintainer email="[email protected]">Khalil Selyan</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>