Skip to content

Commit bc61c9f

Browse files
Merge branch 'main' into test/image_projection_based_fusion/add_calc_iou_tests
2 parents f2fb6e8 + c696905 commit bc61c9f

File tree

262 files changed

+6794
-2155
lines changed

Some content is hidden

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

262 files changed

+6794
-2155
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/.pages

+62
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
nav:
2+
- 'Introduction': common
3+
- 'Testing Libraries':
4+
- 'autoware_testing': common/autoware_testing/design/autoware_testing-design
5+
- 'fake_test_node': common/fake_test_node/design/fake_test_node-design
6+
- 'Common Libraries':
7+
- 'autoware_auto_common':
8+
- 'comparisons': common/autoware_auto_common/design/comparisons
9+
- 'autoware_auto_geometry':
10+
- 'interval': common/autoware_auto_geometry/design/interval
11+
- 'polygon intersection 2d': common/autoware_auto_geometry/design/polygon_intersection_2d-design
12+
- 'spatial hash': common/autoware_auto_geometry/design/spatial-hash-design
13+
- 'autoware_auto_tf2': common/autoware_auto_tf2/design/autoware-auto-tf2-design
14+
- 'autoware_point_types': common/autoware_point_types
15+
- 'Cuda Utils': common/cuda_utils
16+
- 'Geography Utils': common/geography_utils
17+
- 'Global Parameter Loader': common/global_parameter_loader/Readme
18+
- 'Glog Component': common/glog_component
19+
- 'grid_map_utils': common/grid_map_utils
20+
- 'interpolation': common/interpolation
21+
- 'Kalman Filter': common/kalman_filter
22+
- 'Motion Utils': common/motion_utils
23+
- 'Vehicle Utils': common/motion_utils/docs/vehicle/vehicle
24+
- 'Object Recognition Utils': common/object_recognition_utils
25+
- 'OSQP Interface': common/osqp_interface/design/osqp_interface-design
26+
- 'Perception Utils': common/perception_utils
27+
- 'QP Interface': common/qp_interface/design/qp_interface-design
28+
- 'Signal Processing':
29+
- 'Introduction': common/signal_processing
30+
- 'Butterworth Filter': common/signal_processing/documentation/ButterworthFilter
31+
- 'TensorRT Common': common/tensorrt_common
32+
- 'tier4_autoware_utils': common/tier4_autoware_utils
33+
- 'traffic_light_utils': common/traffic_light_utils
34+
- 'TVM Utility':
35+
- 'Introduction': common/tvm_utility
36+
- 'YOLOv2 Tiny Example': common/tvm_utility/tvm-utility-yolo-v2-tiny-tests
37+
- 'RVIZ2 Plugins':
38+
- 'autoware_auto_perception_rviz_plugin': common/autoware_auto_perception_rviz_plugin
39+
- 'autoware_overlay_rviz_plugin': common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin
40+
- 'autoware_mission_details_overlay_rviz_plugin': common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin
41+
- 'bag_time_manager_rviz_plugin': common/bag_time_manager_rviz_plugin
42+
- 'polar_grid': common/polar_grid/Readme
43+
- 'tier4_adapi_rviz_plugin': common/tier4_adapi_rviz_plugin
44+
- 'tier4_api_utils': common/tier4_api_utils
45+
- 'tier4_camera_view_rviz_plugin': common/tier4_camera_view_rviz_plugin
46+
- 'tier4_datetime_rviz_plugin': common/tier4_datetime_rviz_plugin
47+
- 'tier4_localization_rviz_plugin': common/tier4_localization_rviz_plugin
48+
- 'tier4_perception_rviz_plugin': common/tier4_perception_rviz_plugin
49+
- 'tier4_planning_rviz_plugin': common/tier4_planning_rviz_plugin
50+
- 'tier4_state_rviz_plugin': common/tier4_state_rviz_plugin
51+
- 'tier4_system_rviz_plugin': common/tier4_system_rviz_plugin
52+
- 'tier4_traffic_light_rviz_plugin': common/tier4_traffic_light_rviz_plugin
53+
- 'tier4_vehicle_rviz_plugin': common/tier4_vehicle_rviz_plugin
54+
- 'traffic_light_recognition_marker_publisher': common/traffic_light_recognition_marker_publisher/Readme
55+
- 'Node':
56+
- 'Goal Distance Calculator': common/goal_distance_calculator/Readme
57+
- 'Path Distance Calculator': common/path_distance_calculator/Readme
58+
- 'Others':
59+
- 'autoware_ad_api_specs': common/autoware_ad_api_specs
60+
- 'component_interface_specs': common/component_interface_specs
61+
- 'component_interface_tools': common/component_interface_tools
62+
- 'component_interface_utils': common/component_interface_utils

common/README.md

+16
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
# Common
2+
3+
## Getting Started
4+
5+
The Autoware.Universe Common folder consists of common and testing libraries that are used by other Autoware components, as well as useful plugins for visualization in RVIZ2.
6+
7+
!!! note
8+
9+
In addition to the ones listed in this folder, users can also have a look at some of the add-ons in the `autoware_tools/common` documentation [page](https://autowarefoundation.github.io/autoware_tools/main/common/mission_planner_rviz_plugin/).
10+
11+
## Highlights
12+
13+
Some of the commonly used libraries are:
14+
15+
1. `tier4_autoware_utils`
16+
2. `motion_utils`
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/autoware_point_types/README.md

+1
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
# Autoware Point Types

common/component_interface_specs/include/component_interface_specs/localization.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -18,16 +18,16 @@
1818
#include <rclcpp/qos.hpp>
1919

2020
#include <autoware_adapi_v1_msgs/msg/localization_initialization_state.hpp>
21-
#include <autoware_adapi_v1_msgs/srv/initialize_localization.hpp>
2221
#include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
2322
#include <nav_msgs/msg/odometry.hpp>
23+
#include <tier4_localization_msgs/srv/initialize_localization.hpp>
2424

2525
namespace localization_interface
2626
{
2727

2828
struct Initialize
2929
{
30-
using Service = autoware_adapi_v1_msgs::srv::InitializeLocalization;
30+
using Service = tier4_localization_msgs::srv::InitializeLocalization;
3131
static constexpr char name[] = "/localization/initialize";
3232
};
3333

common/component_interface_specs/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
<depend>rclcpp</depend>
2222
<depend>rosidl_runtime_cpp</depend>
2323
<depend>tier4_control_msgs</depend>
24+
<depend>tier4_localization_msgs</depend>
2425
<depend>tier4_map_msgs</depend>
2526
<depend>tier4_planning_msgs</depend>
2627
<depend>tier4_system_msgs</depend>

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

+7-6
Original file line numberDiff line numberDiff line change
@@ -1921,20 +1921,21 @@ insertOrientation<std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>
19211921
* radians (default: M_PI_2)
19221922
*/
19231923
template <class T>
1924-
void removeInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2)
1924+
void removeFirstInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2)
19251925
{
1926-
for (size_t i = 1; i < points.size();) {
1927-
const auto p1 = tier4_autoware_utils::getPose(points.at(i - 1));
1928-
const auto p2 = tier4_autoware_utils::getPose(points.at(i));
1926+
for (auto itr = points.begin(); std::next(itr) != points.end();) {
1927+
const auto p1 = tier4_autoware_utils::getPose(*itr);
1928+
const auto p2 = tier4_autoware_utils::getPose(*std::next(itr));
19291929
const double yaw1 = tf2::getYaw(p1.orientation);
19301930
const double yaw2 = tf2::getYaw(p2.orientation);
19311931

19321932
if (
19331933
max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2)) ||
19341934
!tier4_autoware_utils::isDrivingForward(p1, p2)) {
1935-
points.erase(points.begin() + i);
1935+
points.erase(std::next(itr));
1936+
return;
19361937
} else {
1937-
++i;
1938+
itr++;
19381939
}
19391940
}
19401941
}

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+
}

0 commit comments

Comments
 (0)