Skip to content

Commit cc1ae5f

Browse files
asa-nakih-ohtaito-santakayuki5168KeisukeShima
authored
feat: v0.3.19 rebase (#1540)
* fix(system_monitor): extend command line to display (backport autowarefoundation#4553) (#768) fix(system_monitor): extend command line to display (autowarefoundation#4553) Signed-off-by: ito-san <fumihito.ito@tier4.jp> Co-authored-by: ito-san <57388357+ito-san@users.noreply.github.com> * feat(behavior_path_planner): resample output path (backport #1604) (#782) feat(behavior_path_planner): resample output path (#1604) * feat(behavior_path_planner): resample output path * update param Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com> * ci: add dispatch-push-event workflow (#803) * ci: add dispatch-push-event workflow Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * fix: change APP KEY Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * chore: use strategy Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * chore: change trigger Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * pre-commit fixes Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> * Update .github/workflows/dispatch-push-event.yaml * Update .github/workflows/dispatch-push-event.yaml * style(pre-commit): autofix * Update .github/workflows/dispatch-push-event.yaml --------- Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(behavior_path): only apply spline interpolation for its output, not for turn_signal processing (#909) * fix(behavior_path): only apply spline interpolate for output, not for turn_signal processing * fix implementation * ci(pre-commit): autofix --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(behavior_path): fix base points vanishing and inconsistent lane_ids on the spline interpolated path (#929) * add base points to resampled path in behavior_path * Revert "fix(behavior_path): only apply spline interpolation for its output, not for turn_signal processing (#909)" This reverts commit c80c986. * ci(pre-commit): autofix * fix insert * fix: not interpolate behavior velocity path * Revert "Revert "fix(behavior_path): only apply spline interpolation for its output, not for turn_signal processing (#909)"" This reverts commit e6dd540. --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(system_monitor): fix program command line reading (backport autowarefoundation#5191, autowarefoundation#5430) (#995) * perf(system_monitor): fix program command line reading (autowarefoundation#5191) * Fix program command line reading Signed-off-by: Owen-Liuyuxuan <uken.ryu@tier4.jp> * style(pre-commit): autofix * fix spelling commandline->command_line Signed-off-by: Owen-Liuyuxuan <uken.ryu@tier4.jp> --------- Signed-off-by: Owen-Liuyuxuan <uken.ryu@tier4.jp> Co-authored-by: Owen-Liuyuxuan <uken.ryu@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(system_monitor): output command line (autowarefoundation#5430) * fix(system_monitor): output command line Signed-off-by: takeshi.iwanari <takeshi.iwanari@tier4.jp> * style(pre-commit): autofix --------- Signed-off-by: takeshi.iwanari <takeshi.iwanari@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --------- Signed-off-by: Owen-Liuyuxuan <uken.ryu@tier4.jp> Signed-off-by: takeshi.iwanari <takeshi.iwanari@tier4.jp> Co-authored-by: Yuxuan Liu <619684051@qq.com> Co-authored-by: Owen-Liuyuxuan <uken.ryu@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: takeshi-iwanari <takeshi.iwanari@tier4.jp> Co-authored-by: Akihisa Nagata <54956813+asa-naki@users.noreply.github.com> * feat(obstacle_stop): upd obstacle hunting (#1068) * Adapted from PR #1458 Signed-off-by: Shigekazu Fukuta <shigekazu.fukuta@tier4.jp> * Adapted from PR #1627 Signed-off-by: Shigekazu Fukuta <shigekazu.fukuta@tier4.jp> * fix parameter name Signed-off-by: Shigekazu Fukuta <shigekazu.fukuta@tier4.jp> * Adapted from PR autowarefoundation#2647 Signed-off-by: Shigekazu Fukuta <shigekazu.fukuta@tier4.jp> * ci(pre-commit): autofix * fix build error * ci(pre-commit): autofix * remove comment line * remove logic * Delete parameters other than those added this time * ci(pre-commit): autofix * add stop vehicle check * ci(pre-commit): autofix * fix stop velocity threshold * fix engage obstacle clear and stop threshold * ci(pre-commit): autofix --------- Signed-off-by: Shigekazu Fukuta <shigekazu.fukuta@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(obstacle_avoidance_planner): add empty check (#1285) * fix(obstacle_avoidance_planner): add empty check * ci(pre-commit): autofix * add invalid_argument * delete empty check * return code moved to end * add warning log * update rclcpp_debug * delete debug log * Delete unnecessary blank lines --------- Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> * fix(virtual traffic light): suppress lauch (#1290) * suppress launch Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> * add existence check Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> --------- Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> Co-authored-by: Shigekazu Fukuta <107168699+sfukuta@users.noreply.github.com> * revert: #929,#909,#782 * revert: #1068 - d56c191. --------- Signed-off-by: ito-san <fumihito.ito@tier4.jp> Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Signed-off-by: Owen-Liuyuxuan <uken.ryu@tier4.jp> Signed-off-by: takeshi.iwanari <takeshi.iwanari@tier4.jp> Signed-off-by: Shigekazu Fukuta <shigekazu.fukuta@tier4.jp> Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> Co-authored-by: ito-san <57388357+ito-san@users.noreply.github.com> Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com> Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Yuxuan Liu <619684051@qq.com> Co-authored-by: Owen-Liuyuxuan <uken.ryu@tier4.jp> Co-authored-by: takeshi-iwanari <takeshi.iwanari@tier4.jp> Co-authored-by: Shigekazu Fukuta <107168699+sfukuta@users.noreply.github.com> Co-authored-by: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com>
1 parent a5b62af commit cc1ae5f

File tree

2 files changed

+41
-18
lines changed
  • planning
    • behavior_velocity_planner/src/scene_module/virtual_traffic_light
    • obstacle_avoidance_planner/src

2 files changed

+41
-18
lines changed

‎planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp

+21-1
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,9 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15+
#include "tier4_autoware_utils/geometry/boost_geometry.hpp"
16+
17+
#include <lanelet2_extension/utility/utilities.hpp>
1518
#include <scene_module/virtual_traffic_light/manager.hpp>
1619
#include <tier4_autoware_utils/math/unit_conversion.hpp>
1720

@@ -81,11 +84,28 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node
8184
void VirtualTrafficLightModuleManager::launchNewModules(
8285
const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
8386
{
87+
tier4_autoware_utils::LineString2d ego_path_linestring;
88+
for (const auto & path_point : path.points) {
89+
ego_path_linestring.push_back(
90+
tier4_autoware_utils::fromMsg(path_point.point.pose.position).to_2d());
91+
}
92+
8493
for (const auto & m : getRegElemMapOnPath<VirtualTrafficLight>(
8594
path, planner_data_->route_handler_->getLaneletMapPtr())) {
95+
const auto stop_line_opt = m.first->getStopLine();
96+
if (!stop_line_opt) {
97+
RCLCPP_FATAL(
98+
logger_, "No stop line at virtual_traffic_light_reg_elem_id = %ld, please fix the map!",
99+
m.first->id());
100+
continue;
101+
}
102+
86103
// Use lanelet_id to unregister module when the route is changed
87104
const auto module_id = m.second.id();
88-
if (!isModuleRegistered(module_id)) {
105+
if (
106+
!isModuleRegistered(module_id) &&
107+
boost::geometry::intersects(
108+
ego_path_linestring, lanelet::utils::to2D(stop_line_opt.value()).basicLineString())) {
89109
registerModule(std::make_shared<VirtualTrafficLightModule>(
90110
module_id, *m.first, m.second, planner_param_,
91111
logger_.get_child("virtual_traffic_light_module"), clock_));

‎planning/obstacle_avoidance_planner/src/utils.cpp

+20-17
Original file line numberDiff line numberDiff line change
@@ -343,26 +343,29 @@ std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolate2DTraj
343343
const auto monotonic_base_yaw = convertEulerAngleToMonotonic(base_yaw);
344344

345345
// spline interpolation
346-
const auto interpolated_x = interpolation::slerp(base_s, base_x, new_s);
347-
const auto interpolated_y = interpolation::slerp(base_s, base_y, new_s);
348-
const auto interpolated_yaw = interpolation::slerp(base_s, monotonic_base_yaw, new_s);
349-
350-
for (size_t i = 0; i < interpolated_x.size(); i++) {
351-
if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) {
352-
return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
346+
std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolated_points;
347+
try {
348+
const auto interpolated_x = interpolation::slerp(base_s, base_x, new_s);
349+
const auto interpolated_y = interpolation::slerp(base_s, base_y, new_s);
350+
const auto interpolated_yaw = interpolation::slerp(base_s, monotonic_base_yaw, new_s);
351+
352+
for (size_t i = 0; i < interpolated_x.size(); i++) {
353+
if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) {
354+
return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
355+
}
353356
}
354-
}
355357

356-
std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> interpolated_points;
357-
for (size_t i = 0; i < interpolated_x.size(); i++) {
358-
autoware_auto_planning_msgs::msg::TrajectoryPoint point;
359-
point.pose.position.x = interpolated_x[i];
360-
point.pose.position.y = interpolated_y[i];
361-
point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(
362-
tier4_autoware_utils::normalizeRadian(interpolated_yaw[i]));
363-
interpolated_points.push_back(point);
358+
for (size_t i = 0; i < interpolated_x.size(); i++) {
359+
autoware_auto_planning_msgs::msg::TrajectoryPoint point;
360+
point.pose.position.x = interpolated_x[i];
361+
point.pose.position.y = interpolated_y[i];
362+
point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(
363+
tier4_autoware_utils::normalizeRadian(interpolated_yaw[i]));
364+
interpolated_points.push_back(point);
365+
}
366+
} catch (const std::invalid_argument & e) {
367+
return std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>{};
364368
}
365-
366369
return interpolated_points;
367370
}
368371

0 commit comments

Comments
 (0)