Skip to content

Commit 26505f9

Browse files
committed
feat(autoware_universe): merge branch main into autoware_msg
Signed-off-by: liu cui <cynthia.liu@autocore.ai>
2 parents 49665ae + 7f36c52 commit 26505f9

File tree

206 files changed

+5450
-3240
lines changed

Some content is hidden

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

206 files changed

+5450
-3240
lines changed

.cspell-partial.json

-9
This file was deleted.

.cspell.json

+5
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
{
2+
"ignorePaths": ["perception/bytetrack/lib/**"],
3+
"ignoreRegExpList": [],
4+
"words": ["dltype", "tvmgen"]
5+
}

.github/CODEOWNERS

+3-3
Original file line numberDiff line numberDiff line change
@@ -77,7 +77,7 @@ evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai fumiya.watanabe@tier4
7777
evaluator/localization_evaluator/** dominik.jargot@robotec.ai koji.minoda@tier4.jp
7878
evaluator/perception_online_evaluator/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp
7979
evaluator/planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp
80-
evaluator/tier4_metrics_rviz_plugin/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp
80+
evaluator/tier4_metrics_rviz_plugin/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp
8181
launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp
8282
launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
8383
launch/tier4_localization_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
@@ -121,7 +121,7 @@ perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp
121121
perception/detected_object_validation/** dai.nguyen@tier4.jp shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp
122122
perception/detection_by_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
123123
perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp shintaro.tomie@tier4.jp taichi.higashide@tier4.jp
124-
perception/euclidean_cluster/** yukihiro.saito@tier4.jp
124+
perception/euclidean_cluster/** dai.nguyen@tier4.jp yukihiro.saito@tier4.jp
125125
perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp
126126
perception/image_projection_based_fusion/** dai.nguyen@tier4.jp koji.minoda@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp
127127
perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp
@@ -212,7 +212,7 @@ sensing/image_diagnostics/** dai.nguyen@tier4.jp
212212
sensing/image_transport_decompressor/** yukihiro.saito@tier4.jp
213213
sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
214214
sensing/livox/livox_tag_filter/** ryohsuke.mitsudome@tier4.jp
215-
sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp
215+
sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp
216216
sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
217217
sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp
218218
sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp

.github/workflows/spell-check-all.yaml

-19
This file was deleted.

.github/workflows/spell-check-partial.yaml

+5-2
Original file line numberDiff line numberDiff line change
@@ -2,17 +2,20 @@ name: spell-check-partial
22

33
on:
44
pull_request:
5+
schedule:
6+
- cron: 0 0 * * *
7+
workflow_dispatch:
58

69
jobs:
710
spell-check-partial:
811
runs-on: ubuntu-latest
912
steps:
1013
- name: Check out repository
11-
uses: actions/checkout@v3
14+
uses: actions/checkout@v4
1215

1316
- name: Run spell-check
1417
uses: autowarefoundation/autoware-github-actions/spell-check@v1
1518
with:
1619
cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json
17-
local-cspell-json: .cspell-partial.json
20+
local-cspell-json: .cspell.json
1821
incremental-files-only: false

build_depends.repos

+4
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,10 @@ repositories:
1616
type: git
1717
url: https://github.com/autowarefoundation/autoware_adapi_msgs.git
1818
version: main
19+
core/autoware_internal_msgs:
20+
type: git
21+
url: https://github.com/autowarefoundation/autoware_internal_msgs.git
22+
version: main
1923
core/external/autoware_auto_msgs:
2024
type: git
2125
url: https://github.com/tier4/autoware_auto_msgs.git
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
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 TIER4_AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_
16+
#define TIER4_AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_
17+
18+
#include <rclcpp/rclcpp.hpp>
19+
20+
#include <autoware_internal_msgs/msg/published_time.hpp>
21+
#include <std_msgs/msg/header.hpp>
22+
23+
#include <functional>
24+
#include <map>
25+
#include <string>
26+
27+
namespace tier4_autoware_utils
28+
{
29+
30+
class PublishedTimePublisher
31+
{
32+
public:
33+
explicit PublishedTimePublisher(
34+
rclcpp::Node * node, const std::string & publisher_topic_suffix = "/debug/published_time",
35+
const rclcpp::QoS & qos = rclcpp::QoS(1))
36+
: node_(node), publisher_topic_suffix_(publisher_topic_suffix), qos_(qos)
37+
{
38+
}
39+
40+
void publish_if_subscribed(
41+
const rclcpp::PublisherBase::ConstSharedPtr & publisher, const rclcpp::Time & stamp)
42+
{
43+
const auto & gid_key = publisher->get_gid();
44+
45+
// if the publisher is not in the map, create a new publisher for published time
46+
ensure_publisher_exists(gid_key, publisher->get_topic_name());
47+
48+
const auto & pub_published_time_ = publishers_[gid_key];
49+
50+
// Check if there are any subscribers, otherwise don't do anything
51+
if (pub_published_time_->get_subscription_count() > 0) {
52+
PublishedTime published_time;
53+
54+
published_time.header.stamp = stamp;
55+
published_time.published_stamp = rclcpp::Clock().now();
56+
57+
pub_published_time_->publish(published_time);
58+
}
59+
}
60+
61+
void publish_if_subscribed(
62+
const rclcpp::PublisherBase::ConstSharedPtr & publisher, const std_msgs::msg::Header & header)
63+
{
64+
const auto & gid_key = publisher->get_gid();
65+
66+
// if the publisher is not in the map, create a new publisher for published time
67+
ensure_publisher_exists(gid_key, publisher->get_topic_name());
68+
69+
const auto & pub_published_time_ = publishers_[gid_key];
70+
71+
// Check if there are any subscribers, otherwise don't do anything
72+
if (pub_published_time_->get_subscription_count() > 0) {
73+
PublishedTime published_time;
74+
75+
published_time.header = header;
76+
published_time.published_stamp = rclcpp::Clock().now();
77+
78+
pub_published_time_->publish(published_time);
79+
}
80+
}
81+
82+
private:
83+
rclcpp::Node * node_;
84+
std::string publisher_topic_suffix_;
85+
rclcpp::QoS qos_;
86+
87+
using PublishedTime = autoware_internal_msgs::msg::PublishedTime;
88+
89+
// Custom comparison struct for rmw_gid_t
90+
struct GidCompare
91+
{
92+
bool operator()(const rmw_gid_t & lhs, const rmw_gid_t & rhs) const
93+
{
94+
return std::memcmp(lhs.data, rhs.data, RMW_GID_STORAGE_SIZE) < 0;
95+
}
96+
};
97+
98+
// ensure that the publisher exists in publisher_ map, if not, create a new one
99+
void ensure_publisher_exists(const rmw_gid_t & gid_key, const std::string & topic_name)
100+
{
101+
if (publishers_.find(gid_key) == publishers_.end()) {
102+
publishers_[gid_key] =
103+
node_->create_publisher<PublishedTime>(topic_name + publisher_topic_suffix_, qos_);
104+
}
105+
}
106+
107+
// store them for each different publisher of the node
108+
std::map<rmw_gid_t, rclcpp::Publisher<PublishedTime>::SharedPtr, GidCompare> publishers_;
109+
};
110+
} // namespace tier4_autoware_utils
111+
112+
#endif // TIER4_AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_

common/tier4_autoware_utils/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
<depend>autoware_perception_msgs</depend>
1616
<depend>autoware_planning_msgs</depend>
1717
<depend>autoware_vehicle_msgs</depend>
18+
<depend>autoware_internal_msgs</depend>
1819
<depend>builtin_interfaces</depend>
1920
<depend>diagnostic_msgs</depend>
2021
<depend>geometry_msgs</depend>

control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -121,7 +121,7 @@ class LaneDepartureChecker
121121
std::vector<std::pair<double, lanelet::Lanelet>> getLaneletsFromPath(
122122
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
123123

124-
std::optional<lanelet::BasicPolygon2d> getFusedLaneletPolygonForPath(
124+
std::optional<tier4_autoware_utils::Polygon2d> getFusedLaneletPolygonForPath(
125125
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const;
126126

127127
bool checkPathWillLeaveLane(

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp

+25-19
Original file line numberDiff line numberDiff line change
@@ -321,33 +321,39 @@ std::vector<std::pair<double, lanelet::Lanelet>> LaneDepartureChecker::getLanele
321321
lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0);
322322
}
323323

324-
std::optional<lanelet::BasicPolygon2d> LaneDepartureChecker::getFusedLaneletPolygonForPath(
324+
std::optional<tier4_autoware_utils::Polygon2d> LaneDepartureChecker::getFusedLaneletPolygonForPath(
325325
const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const
326326
{
327327
const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path);
328+
auto to_polygon2d = [](const lanelet::BasicPolygon2d & poly) -> tier4_autoware_utils::Polygon2d {
329+
tier4_autoware_utils::Polygon2d p;
330+
auto & outer = p.outer();
331+
332+
for (const auto & p : poly) {
333+
tier4_autoware_utils::Point2d p2d(p.x(), p.y());
334+
outer.push_back(p2d);
335+
}
336+
boost::geometry::correct(p);
337+
return p;
338+
};
339+
328340
// Fuse lanelets into a single BasicPolygon2d
329-
auto fused_lanelets = [&lanelets_distance_pair]() -> std::optional<lanelet::BasicPolygon2d> {
341+
auto fused_lanelets = [&]() -> std::optional<tier4_autoware_utils::Polygon2d> {
330342
if (lanelets_distance_pair.empty()) return std::nullopt;
331-
if (lanelets_distance_pair.size() == 1)
332-
return lanelets_distance_pair.at(0).second.polygon2d().basicPolygon();
343+
tier4_autoware_utils::MultiPolygon2d lanelet_unions;
344+
tier4_autoware_utils::MultiPolygon2d result;
333345

334-
lanelet::BasicPolygon2d merged_polygon =
335-
lanelets_distance_pair.at(0).second.polygon2d().basicPolygon();
336-
for (size_t i = 1; i < lanelets_distance_pair.size(); ++i) {
346+
for (size_t i = 0; i < lanelets_distance_pair.size(); ++i) {
337347
const auto & route_lanelet = lanelets_distance_pair.at(i).second;
338-
const auto & poly = route_lanelet.polygon2d().basicPolygon();
339-
340-
std::vector<lanelet::BasicPolygon2d> lanelet_union_temp;
341-
boost::geometry::union_(poly, merged_polygon, lanelet_union_temp);
342-
343-
// Update merged_polygon by accumulating all merged results
344-
merged_polygon.clear();
345-
for (const auto & temp_poly : lanelet_union_temp) {
346-
merged_polygon.insert(merged_polygon.end(), temp_poly.begin(), temp_poly.end());
347-
}
348+
const auto & p = route_lanelet.polygon2d().basicPolygon();
349+
tier4_autoware_utils::Polygon2d poly = to_polygon2d(p);
350+
boost::geometry::union_(lanelet_unions, poly, result);
351+
lanelet_unions = result;
352+
result.clear();
348353
}
349-
if (merged_polygon.empty()) return std::nullopt;
350-
return merged_polygon;
354+
355+
if (lanelet_unions.empty()) return std::nullopt;
356+
return lanelet_unions.front();
351357
}();
352358

353359
return fused_lanelets;

control/mpc_lateral_controller/src/mpc.cpp

+5-1
Original file line numberDiff line numberDiff line change
@@ -355,12 +355,15 @@ std::pair<bool, VectorXd> MPC::updateStateForDelayCompensation(
355355
MatrixXd Wd(DIM_X, 1);
356356
MatrixXd Cd(DIM_Y, DIM_X);
357357

358+
const double sign_vx = m_is_forward_shift ? 1 : -1;
359+
358360
MatrixXd x_curr = x0_orig;
359361
double mpc_curr_time = start_time;
360362
for (size_t i = 0; i < m_input_buffer.size(); ++i) {
361363
double k, v = 0.0;
362364
try {
363-
k = interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time);
365+
// NOTE: When driving backward, the curvature's sign should be reversed.
366+
k = interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time) * sign_vx;
364367
v = interpolation::lerp(traj.relative_time, traj.vx, mpc_curr_time);
365368
} catch (const std::exception & e) {
366369
RCLCPP_ERROR(m_logger, "mpc resample failed at delay compensation, stop mpc: %s", e.what());
@@ -446,6 +449,7 @@ MPCMatrix MPC::generateMPCMatrix(
446449
const double ref_vx = reference_trajectory.vx.at(i);
447450
const double ref_vx_squared = ref_vx * ref_vx;
448451

452+
// NOTE: When driving backward, the curvature's sign should be reversed.
449453
const double ref_k = reference_trajectory.k.at(i) * sign_vx;
450454
const double ref_smooth_k = reference_trajectory.smooth_k.at(i) * sign_vx;
451455

control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp

+11-1
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525
#include "tf2/utils.h"
2626
#include "tf2_ros/buffer.h"
2727
#include "tf2_ros/transform_listener.h"
28+
#include "tier4_autoware_utils/ros/marker_helper.hpp"
2829
#include "trajectory_follower_base/longitudinal_controller_base.hpp"
2930
#include "vehicle_info_util/vehicle_info_util.hpp"
3031

@@ -39,6 +40,7 @@
3940
#include "nav_msgs/msg/odometry.hpp"
4041
#include "tf2_msgs/msg/tf_message.hpp"
4142
#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp"
43+
#include "visualization_msgs/msg/marker.hpp"
4244

4345
#include <deque>
4446
#include <memory>
@@ -50,6 +52,11 @@
5052
namespace autoware::motion::control::pid_longitudinal_controller
5153
{
5254
using autoware_adapi_v1_msgs::msg::OperationModeState;
55+
using tier4_autoware_utils::createDefaultMarker;
56+
using tier4_autoware_utils::createMarkerColor;
57+
using tier4_autoware_utils::createMarkerScale;
58+
using visualization_msgs::msg::Marker;
59+
5360
namespace trajectory_follower = ::autoware::motion::control::trajectory_follower;
5461

5562
/// \class PidLongitudinalController
@@ -97,6 +104,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
97104
// ros variables
98105
rclcpp::Publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr m_pub_slope;
99106
rclcpp::Publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr m_pub_debug;
107+
rclcpp::Publisher<Marker>::SharedPtr m_pub_stop_reason_marker;
100108

101109
rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res;
102110
rcl_interfaces::msg::SetParametersResult paramCallback(
@@ -109,7 +117,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
109117
OperationModeState m_current_operation_mode;
110118

111119
// vehicle info
112-
double m_wheel_base;
120+
double m_wheel_base{0.0};
121+
double m_vehicle_width{0.0};
122+
double m_front_overhang{0.0};
113123
bool m_prev_vehicle_is_under_control{false};
114124
std::shared_ptr<rclcpp::Time> m_under_control_starting_time{nullptr};
115125

0 commit comments

Comments
 (0)