Skip to content

Commit 4e7a31e

Browse files
cyn-liucyn-liu
committed
merge branch main into autoware_msg
Signed-off-by: liu cui <cynthia.liu@autocore.ai>
2 parents ce92b5d + 84e33d8 commit 4e7a31e

File tree

243 files changed

+5977
-1988
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

243 files changed

+5977
-1988
lines changed

‎.github/CODEOWNERS

+3-13
Original file line numberDiff line numberDiff line change
@@ -21,35 +21,25 @@ common/goal_distance_calculator/** taiki.tanaka@tier4.jp
2121
common/grid_map_utils/** maxime.clement@tier4.jp
2222
common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp
2323
common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp
24-
common/mission_planner_rviz_plugin/** isamu.takagi@tier4.jp
2524
common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
2625
common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp
2726
common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp
2827
common/path_distance_calculator/** isamu.takagi@tier4.jp
2928
common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp
3029
common/polar_grid/** yukihiro.saito@tier4.jp
3130
common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp
32-
common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp
3331
common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
3432
common/tensorrt_common/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp
3533
common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp
3634
common/tier4_api_utils/** isamu.takagi@tier4.jp
37-
common/tier4_automatic_goal_rviz_plugin/** dawid.moszynski@robotec.ai kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp
3835
common/tier4_autoware_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
39-
common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp
4036
common/tier4_camera_view_rviz_plugin/** makoto.ybauta@tier4.jp uken.ryu@tier4.jp
41-
common/tier4_control_rviz_plugin/** taiki.tanaka@tier4.jp
4237
common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp
43-
common/tier4_debug_rviz_plugin/** takayuki.murooka@tier4.jp
4438
common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp takamasa.horibe@tier4.jp
45-
common/tier4_logging_level_configure_rviz_plugin/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp
4639
common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp
4740
common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp
48-
common/tier4_screen_capture_rviz_plugin/** kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp
49-
common/tier4_simulated_clock_rviz_plugin/** maxime.clement@tier4.jp
50-
common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp
41+
common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp khalil@leodrive.ai
5142
common/tier4_system_rviz_plugin/** koji.minoda@tier4.jp
52-
common/tier4_target_object_type_rviz_plugin/** takamasa.horibe@tier4.jp
5343
common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp
5444
common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp
5545
common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
@@ -156,13 +146,14 @@ perception/traffic_light_map_based_detector/** shunsuke.miura@tier4.jp tao.zhong
156146
perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp
157147
perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp
158148
perception/traffic_light_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier4.jp
149+
planning/autoware_behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
150+
planning/autoware_behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp
159151
planning/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp
160152
planning/autoware_remaining_distance_time_calculator/** ahmed.ebrahim@leodrive.ai
161153
planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp
162154
planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
163155
planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
164156
planning/behavior_path_dynamic_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp
165-
planning/behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
166157
planning/behavior_path_goal_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp
167158
planning/behavior_path_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
168159
planning/behavior_path_planner/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp
@@ -179,7 +170,6 @@ planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai
179170
planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
180171
planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp
181172
planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
182-
planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp
183173
planning/behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
184174
planning/behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp
185175
planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp

‎common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp

+36
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
// Copyright 2024 The Autoware Contributors
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE_AD_API_SPECS__SYSTEM_HPP_
16+
#define AUTOWARE_AD_API_SPECS__SYSTEM_HPP_
17+
18+
#include <rclcpp/qos.hpp>
19+
20+
#include <autoware_adapi_v1_msgs/msg/heartbeat.hpp>
21+
22+
namespace autoware_ad_api::system
23+
{
24+
25+
struct Heartbeat
26+
{
27+
using Message = autoware_adapi_v1_msgs::msg::Heartbeat;
28+
static constexpr char name[] = "/api/system/heartbeat";
29+
static constexpr size_t depth = 10;
30+
static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
31+
static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
32+
};
33+
34+
} // namespace autoware_ad_api::system
35+
36+
#endif // AUTOWARE_AD_API_SPECS__SYSTEM_HPP_

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

+7-6
Original file line numberDiff line numberDiff line change
@@ -1903,20 +1903,21 @@ extern template void insertOrientation<std::vector<autoware_planning_msgs::msg::
19031903
* radians (default: M_PI_2)
19041904
*/
19051905
template <class T>
1906-
void removeInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2)
1906+
void removeFirstInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2)
19071907
{
1908-
for (size_t i = 1; i < points.size();) {
1909-
const auto p1 = tier4_autoware_utils::getPose(points.at(i - 1));
1910-
const auto p2 = tier4_autoware_utils::getPose(points.at(i));
1908+
for (auto itr = points.begin(); std::next(itr) != points.end();) {
1909+
const auto p1 = tier4_autoware_utils::getPose(*itr);
1910+
const auto p2 = tier4_autoware_utils::getPose(*std::next(itr));
19111911
const double yaw1 = tf2::getYaw(p1.orientation);
19121912
const double yaw2 = tf2::getYaw(p2.orientation);
19131913

19141914
if (
19151915
max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2)) ||
19161916
!tier4_autoware_utils::isDrivingForward(p1, p2)) {
1917-
points.erase(points.begin() + i);
1917+
points.erase(std::next(itr));
1918+
return;
19181919
} else {
1919-
++i;
1920+
itr++;
19201921
}
19211922
}
19221923
}

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

+5-16
Original file line numberDiff line numberDiff line change
@@ -5337,10 +5337,10 @@ TEST(trajectory, cropPoints)
53375337
}
53385338
}
53395339

5340-
TEST(Trajectory, removeInvalidOrientationPoints)
5340+
TEST(Trajectory, removeFirstInvalidOrientationPoints)
53415341
{
53425342
using motion_utils::insertOrientation;
5343-
using motion_utils::removeInvalidOrientationPoints;
5343+
using motion_utils::removeFirstInvalidOrientationPoints;
53445344

53455345
const double max_yaw_diff = M_PI_2;
53465346

@@ -5351,7 +5351,7 @@ TEST(Trajectory, removeInvalidOrientationPoints)
53515351
auto modified_traj = traj;
53525352
insertOrientation(modified_traj.points, true);
53535353
modifyTrajectory(modified_traj);
5354-
removeInvalidOrientationPoints(modified_traj.points, max_yaw_diff);
5354+
removeFirstInvalidOrientationPoints(modified_traj.points, max_yaw_diff);
53555355
EXPECT_EQ(modified_traj.points.size(), expected_size);
53565356
for (size_t i = 0; i < modified_traj.points.size() - 1; ++i) {
53575357
EXPECT_EQ(traj.points.at(i), modified_traj.points.at(i));
@@ -5374,7 +5374,7 @@ TEST(Trajectory, removeInvalidOrientationPoints)
53745374
[&](Trajectory & t) {
53755375
auto invalid_point = t.points.back();
53765376
invalid_point.pose.orientation =
5377-
tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2));
5377+
tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_4));
53785378
t.points.push_back(invalid_point);
53795379
},
53805380
traj.points.size());
@@ -5385,21 +5385,10 @@ TEST(Trajectory, removeInvalidOrientationPoints)
53855385
[&](Trajectory & t) {
53865386
auto invalid_point = t.points[4];
53875387
invalid_point.pose.orientation =
5388-
tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2));
5388+
tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_4));
53895389
t.points.insert(t.points.begin() + 4, invalid_point);
53905390
},
53915391
traj.points.size());
5392-
5393-
// invalid point at the beginning
5394-
testRemoveInvalidOrientationPoints(
5395-
traj,
5396-
[&](Trajectory & t) {
5397-
auto invalid_point = t.points.front();
5398-
invalid_point.pose.orientation =
5399-
tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3 * M_PI_2));
5400-
t.points.insert(t.points.begin(), invalid_point);
5401-
},
5402-
1); // expected size is 1 since only the first point remains
54035392
}
54045393

54055394
TEST(trajectory, calcYawDeviation)

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

+4
Original file line numberDiff line numberDiff line change
@@ -15,13 +15,17 @@
1515
#ifndef TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
1616
#define TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
1717

18+
#include <utility>
19+
1820
namespace tier4_autoware_utils
1921
{
2022

2123
float sin(float radian);
2224

2325
float cos(float radian);
2426

27+
std::pair<float, float> sin_and_cos(float radian);
28+
2529
} // namespace tier4_autoware_utils
2630

2731
#endif // TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_

‎common/tier4_autoware_utils/src/math/trigonometry.cpp

+23
Original file line numberDiff line numberDiff line change
@@ -49,4 +49,27 @@ float cos(float radian)
4949
return sin(radian + static_cast<float>(tier4_autoware_utils::pi) / 2.f);
5050
}
5151

52+
std::pair<float, float> sin_and_cos(float radian)
53+
{
54+
constexpr float tmp =
55+
(180.f / static_cast<float>(tier4_autoware_utils::pi)) * (discrete_arcs_num_360 / 360.f);
56+
const float degree = radian * tmp;
57+
size_t idx =
58+
(static_cast<int>(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) %
59+
discrete_arcs_num_360;
60+
61+
if (idx < discrete_arcs_num_90) {
62+
return {g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]};
63+
} else if (discrete_arcs_num_90 <= idx && idx < 2 * discrete_arcs_num_90) {
64+
idx = 2 * discrete_arcs_num_90 - idx;
65+
return {g_sin_table[idx], -g_sin_table[discrete_arcs_num_90 - idx]};
66+
} else if (2 * discrete_arcs_num_90 <= idx && idx < 3 * discrete_arcs_num_90) {
67+
idx = idx - 2 * discrete_arcs_num_90;
68+
return {-g_sin_table[idx], -g_sin_table[discrete_arcs_num_90 - idx]};
69+
} else { // 3 * discrete_arcs_num_90 <= idx && idx < 4 * discrete_arcs_num_90
70+
idx = 4 * discrete_arcs_num_90 - idx;
71+
return {-g_sin_table[idx], g_sin_table[discrete_arcs_num_90 - idx]};
72+
}
73+
}
74+
5275
} // namespace tier4_autoware_utils

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

+10
Original file line numberDiff line numberDiff line change
@@ -40,3 +40,13 @@ TEST(trigonometry, cos)
4040
tier4_autoware_utils::cos(x * static_cast<float>(i))) < 10e-5);
4141
}
4242
}
43+
44+
TEST(trigonometry, sin_and_cos)
45+
{
46+
float x = 4.f * tier4_autoware_utils::pi / 128.f;
47+
for (int i = 0; i < 128; i++) {
48+
const auto sin_and_cos = tier4_autoware_utils::sin_and_cos(x * static_cast<float>(i));
49+
EXPECT_TRUE(std::abs(std::sin(x * static_cast<float>(i)) - sin_and_cos.first) < 10e-7);
50+
EXPECT_TRUE(std::abs(std::cos(x * static_cast<float>(i)) - sin_and_cos.second) < 10e-7);
51+
}
52+
}

‎common/tier4_state_rviz_plugin/CMakeLists.txt

+27
Original file line numberDiff line numberDiff line change
@@ -13,15 +13,42 @@ add_definitions(-DQT_NO_KEYWORDS)
1313
ament_auto_add_library(${PROJECT_NAME} SHARED
1414
src/autoware_state_panel.cpp
1515
src/velocity_steering_factors_panel.cpp
16+
src/custom_toggle_switch.cpp
17+
src/custom_slider.cpp
18+
src/custom_container.cpp
19+
src/custom_button.cpp
20+
src/custom_icon_label.cpp
21+
src/custom_segmented_button.cpp
22+
src/custom_segmented_button_item.cpp
23+
src/custom_label.cpp
24+
src/include/material_colors.hpp
25+
src/include/autoware_state_panel.hpp
26+
src/include/custom_button.hpp
27+
src/include/custom_container.hpp
28+
src/include/custom_icon_label.hpp
29+
src/include/custom_label.hpp
30+
src/include/custom_segmented_button_item.hpp
31+
src/include/custom_segmented_button.hpp
32+
src/include/custom_slider.hpp
33+
src/include/custom_toggle_switch.hpp
34+
src/include/velocity_steering_factors_panel.hpp
1635
)
1736

37+
target_include_directories(
38+
${PROJECT_NAME} PUBLIC
39+
)
40+
41+
1842
target_link_libraries(${PROJECT_NAME}
1943
${QT_LIBRARIES}
2044
)
2145

2246
# Export the plugin to be imported by rviz2
2347
pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml)
2448

49+
50+
51+
2552
ament_auto_package(
2653
INSTALL_TO_SHARE
2754
icons

‎common/tier4_state_rviz_plugin/icons/assets/active.png

739 Bytes
Loading

‎common/tier4_state_rviz_plugin/icons/assets/crash.png

1.25 KB
Loading

‎common/tier4_state_rviz_plugin/icons/assets/danger.png

808 Bytes
Loading

‎common/tier4_state_rviz_plugin/icons/assets/none.png

531 Bytes
Loading

‎common/tier4_state_rviz_plugin/icons/assets/pending.png

753 Bytes
Loading

‎common/tier4_state_rviz_plugin/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
<description>The autoware state rviz plugin package</description>
77
<maintainer email="hiroki.ota@tier4.jp">Hiroki OTA</maintainer>
88
<maintainer email="isamu.takagi@tier4.jp">Takagi, Isamu</maintainer>
9+
<maintainer email="khalil@leodrive.ai">Khalil Selyan</maintainer>
910
<license>Apache License 2.0</license>
1011

1112
<buildtool_depend>ament_cmake_auto</buildtool_depend>

0 commit comments

Comments
 (0)